From 12204e42dd09d32e40ffbcbd34194ec9a80cae22 Mon Sep 17 00:00:00 2001 From: prw4s76gk <2107884590@qq.com> Date: Fri, 17 May 2024 09:40:07 +0800 Subject: [PATCH 1/9] ADD file via upload --- src/pair | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/pair diff --git a/src/pair b/src/pair new file mode 100644 index 0000000..e69de29 From a529e8291263447531b03b27b0e232e9925e0be6 Mon Sep 17 00:00:00 2001 From: prw4s76gk <2107884590@qq.com> Date: Fri, 17 May 2024 09:40:23 +0800 Subject: [PATCH 2/9] Delete 'src/pair' --- src/pair | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/pair diff --git a/src/pair b/src/pair deleted file mode 100644 index e69de29..0000000 From f6130b2d1f48362051f9bdb77ae490650aa3e62c Mon Sep 17 00:00:00 2001 From: laptoy <2107884590@qq.com> Date: Thu, 6 Jun 2024 15:33:06 +0800 Subject: [PATCH 3/9] change --- src/GPS/GPSManager.cc | 9 +- src/GPS/GPSManager.h | 8 - src/GPS/GPSPositionMessage.h | 9 +- src/GPS/GPSProvider.cc | 9 +- src/GPS/GPSProvider.h | 9 - src/GPS/RTCM/RTCMMavlink.cc | 9 +- src/GPS/RTCM/RTCMMavlink.h | 9 +- src/GPS/definitions.h | 33 +-- src/GPS/satellite_info.h | 33 +-- src/GPS/vehicle_gps_position.h | 33 --- src/fhog.cpp | 259 ++++++++++++++--------- src/kcf.cpp | 363 +++++++++++++++++++++++++++++++++ src/kcftracker.cpp | 293 ++++++++++---------------- 13 files changed, 637 insertions(+), 439 deletions(-) create mode 100644 src/kcf.cpp diff --git a/src/GPS/GPSManager.cc b/src/GPS/GPSManager.cc index a4ee82d..4c90fea 100644 --- a/src/GPS/GPSManager.cc +++ b/src/GPS/GPSManager.cc @@ -1,11 +1,4 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ + #include "GPSManager.h" diff --git a/src/GPS/GPSManager.h b/src/GPS/GPSManager.h index eb22642..dffa72e 100644 --- a/src/GPS/GPSManager.h +++ b/src/GPS/GPSManager.h @@ -1,11 +1,3 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ #pragma once diff --git a/src/GPS/GPSPositionMessage.h b/src/GPS/GPSPositionMessage.h index a2437f3..3f5ba29 100644 --- a/src/GPS/GPSPositionMessage.h +++ b/src/GPS/GPSPositionMessage.h @@ -1,11 +1,4 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ + #pragma once diff --git a/src/GPS/GPSProvider.cc b/src/GPS/GPSProvider.cc index 16b84ed..e74fdc1 100644 --- a/src/GPS/GPSProvider.cc +++ b/src/GPS/GPSProvider.cc @@ -1,11 +1,4 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ + #include "GPSProvider.h" diff --git a/src/GPS/GPSProvider.h b/src/GPS/GPSProvider.h index 3b69a1c..a901f8b 100644 --- a/src/GPS/GPSProvider.h +++ b/src/GPS/GPSProvider.h @@ -1,12 +1,3 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - #pragma once diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc index f859a9a..19dedd7 100644 --- a/src/GPS/RTCM/RTCMMavlink.cc +++ b/src/GPS/RTCM/RTCMMavlink.cc @@ -1,11 +1,4 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ + #include "RTCMMavlink.h" diff --git a/src/GPS/RTCM/RTCMMavlink.h b/src/GPS/RTCM/RTCMMavlink.h index 14b649d..a5d4bbc 100644 --- a/src/GPS/RTCM/RTCMMavlink.h +++ b/src/GPS/RTCM/RTCMMavlink.h @@ -1,11 +1,4 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ + #pragma once diff --git a/src/GPS/definitions.h b/src/GPS/definitions.h index 9267574..b4377d3 100644 --- a/src/GPS/definitions.h +++ b/src/GPS/definitions.h @@ -1,35 +1,4 @@ -/**************************************************************************** - * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ + /** * @file definitions.h diff --git a/src/GPS/satellite_info.h b/src/GPS/satellite_info.h index 2731319..e2d1450 100644 --- a/src/GPS/satellite_info.h +++ b/src/GPS/satellite_info.h @@ -1,35 +1,4 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ + #pragma once #include diff --git a/src/GPS/vehicle_gps_position.h b/src/GPS/vehicle_gps_position.h index 3d1e250..3e2b589 100644 --- a/src/GPS/vehicle_gps_position.h +++ b/src/GPS/vehicle_gps_position.h @@ -1,37 +1,4 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/* Auto-generated by genmsg_cpp from file /home/beat/px4/src/Firmware/msg/vehicle_gps_position.msg */ #pragma once diff --git a/src/fhog.cpp b/src/fhog.cpp index c990823..3fa75c5 100644 --- a/src/fhog.cpp +++ b/src/fhog.cpp @@ -1,43 +1,3 @@ -/*M/////////////////////////////////////////////////////////////////////////////////////// -// -// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. -// -// By downloading, copying, installing or using the software you agree to this license. -// If you do not agree to this license, do not download, install, -// copy or use the software. -// -// -// License Agreement -// For Open Source Computer Vision Library -// -// Copyright (C) 2010-2013, University of Nizhny Novgorod, all rights reserved. -// Third party copyrights are property of their respective owners. -// -// Redistribution and use in source and binary forms, with or without modification, -// are permitted provided that the following conditions are met: -// -// * Redistribution's of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// * Redistribution's in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * The name of the copyright holders may not be used to endorse or promote products -// derived from this software without specific prior written permission. -// -// This software is provided by the copyright holders and contributors "as is" and -// any express or implied warranties, including, but not limited to, the implied -// warranties of merchantability and fitness for a particular purpose are disclaimed. -// In no event shall the Intel Corporation or contributors be liable for any direct, -// indirect, incidental, special, exemplary, or consequential damages -// (including, but not limited to, procurement of substitute goods or services; -// loss of use, data, or profits; or business interruption) however caused -// and on any theory of liability, whether in contract, strict liability, -// or tort (including negligence or otherwise) arising in any way out of -// the use of this software, even if advised of the possibility of such damage. -// -//M*/ //Modified from latentsvm module's "lsvmc_featurepyramid.cpp". @@ -77,60 +37,92 @@ // RESULT // Error status */ +// 从输入的图像中提取特征映射 int getFeatureMaps(const IplImage* image, const int k, CvLSVMFeatureMapCaskade **map) { + // 图像的宽度和高度 int sizeX, sizeY; - int p, px, stringSize; + // 每个cell中的特征数量 + int p, px; + // 存储特征的字符串大小 + int stringSize; + // 图像的高度、宽度和通道数 int height, width, numChannels; + // 循环变量 int i, j, kk, c, ii, jj, d; + // 梯度数据指针 float * datadx, * datady; - - int ch; + + // 角度相关变量 + int ch; float magnitude, x, y, tx, ty; - + + // 梯度图像 IplImage * dx, * dy; + // 最近的邻域点索引 int *nearest; - float *w, a_x, b_x; + // 权重向量 + float *w; + // 线性插值系数 + float a_x, b_x; + // 梯度算子 float kernel[3] = {-1.f, 0.f, 1.f}; + // OpenCV矩阵格式的梯度算子 CvMat kernel_dx = cvMat(1, 3, CV_32F, kernel); CvMat kernel_dy = cvMat(3, 1, CV_32F, kernel); + // 临时指针 float * r; + // 未知用途的整型数组（可能是用于后续处理的索引或标志） int * alfa; - + + // 边界角度值数组 float boundary_x[NUM_SECTOR + 1]; float boundary_y[NUM_SECTOR + 1]; + // 用于查找最大值的变量 float max, dotProd; int maxi; + // 获取图像的高度和宽度 height = image->height; - width = image->width ; + width = image->width; + // 获取图像的通道数 numChannels = image->nChannels; - dx = cvCreateImage(cvSize(image->width, image->height), + // 创建用于存储梯度图像的容器 + dx = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_32F, 3); - dy = cvCreateImage(cvSize(image->width, image->height), + dy = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_32F, 3); + // 计算特征映射的宽和高（基于k的大小） sizeX = width / k; sizeY = height / k; - px = 3 * NUM_SECTOR; + // 计算每个cell中的特征数量（基于NUM_SECTOR） + px = 3 * NUM_SECTOR; p = px; + // 计算整个特征映射所需的总存储大小 stringSize = sizeX * p; + // 分配特征映射对象的内存 allocFeatureMapObject(map, sizeX, sizeY, p); + // 使用Sobel算子计算图像的梯度 cvFilter2D(image, dx, &kernel_dx, cvPoint(-1, 0)); cvFilter2D(image, dy, &kernel_dy, cvPoint(0, -1)); - + + // 计算边界角度值 float arg_vector; for(i = 0; i <= NUM_SECTOR; i++) { + // 计算角度（基于NUM_SECTOR将圆周等分为NUM_SECTOR个部分） arg_vector = ( (float) i ) * ( (float)(PI) / (float)(NUM_SECTOR) ); + // 计算对应的余弦和正弦值 boundary_x[i] = cosf(arg_vector); boundary_y[i] = sinf(arg_vector); - }/*for(i = 0; i <= NUM_SECTOR; i++) */ + } + r = (float *)malloc( sizeof(float) * (width * height)); alfa = (int *)malloc( sizeof(int ) * (width * height * 2)); @@ -287,43 +279,63 @@ int getFeatureMaps(const IplImage* image, const int k, CvLSVMFeatureMapCaskade * // RESULT // Error status */ +// 对特征映射进行归一化和截断处理 int normalizeAndTruncate(CvLSVMFeatureMapCaskade *map, const float alfa) { - int i,j, ii; - int sizeX, sizeY, p, pos, pp, xp, pos1, pos2; - float * partOfNorm; // norm of C(i, j) - float * newData; - float valOfNorm; + int i, j, ii; // 定义循环变量 + int sizeX, sizeY, p, pos, pp, xp; // 定义变量 + float * partOfNorm; // 存储C(i, j)的范数的指针 + float * newData; // 存储归一化后数据的指针 + float valOfNorm; // 存储范数的值 - sizeX = map->sizeX; - sizeY = map->sizeY; - partOfNorm = (float *)malloc (sizeof(float) * (sizeX * sizeY)); + sizeX = map->sizeX; // 获取特征映射的宽度 + sizeY = map->sizeY; // 获取特征映射的高度 + partOfNorm = (float *)malloc (sizeof(float) * (sizeX * sizeY)); // 分配存储范数的内存 - p = NUM_SECTOR; - xp = NUM_SECTOR * 3; - pp = NUM_SECTOR * 12; + p = NUM_SECTOR; // 定义一个常量，可能表示特征的分区数 + xp = NUM_SECTOR * 3; // 定义一个常量，可能表示特征的某种属性 + pp = NUM_SECTOR * 12; // 定义一个常量，可能表示特征的某种属性 + // 计算每个部分区域的范数 for(i = 0; i < sizeX * sizeY; i++) { - valOfNorm = 0.0f; - pos = i * map->numFeatures; - for(j = 0; j < p; j++) + valOfNorm = 0.0f; // 初始化范数为0 + pos = i * map->numFeatures; // 计算每个部分的起始位置 + for(j = 0; j < p; j++) // 遍历每个部分的特征 { - valOfNorm += map->map[pos + j] * map->map[pos + j]; + valOfNorm += map->map[pos + j] * map->map[pos + j]; // 计算每个部分的范数 }/*for(j = 0; j < p; j++)*/ - partOfNorm[i] = valOfNorm; + partOfNorm[i] = valOfNorm; // 将每个部分的范数存储起来 }/*for(i = 0; i < sizeX * sizeY; i++)*/ - + + // 调整特征映射的大小 sizeX -= 2; sizeY -= 2; + // 分配存储归一化后数据的内存 newData = (float *)malloc (sizeof(float) * (sizeX * sizeY * pp)); -//normalization + + // 归一化处理 for(i = 1; i <= sizeY; i++) { for(j = 1; j <= sizeX; j++) { - valOfNorm = sqrtf( + valOfNorm = sqrtf( // 计算当前像素的范数 + partOfNorm[(i )*(sizeX + 2) + (j )] + + partOfNorm[(i )*(sizeX + 2) + (j + 1)] + + partOfNorm[(i + 1)*(sizeX + 2) + (j )] + + partOfNorm[(i + 1)*(sizeX + 2) + (j + 1)]) + FLT_EPSILON; + pos1 = (i ) * (sizeX + 2) * xp + (j ) * xp; + pos2 = (i-1) * (sizeX ) * pp + (j-1) * pp; + for(ii = 0; ii < p; ii++) + { + newData[pos2 + ii ] = map->map[pos1 + ii ] / valOfNorm; // 归一化 + }/*for(ii = 0; ii < p; ii++)*/ + for(ii = 0; ii < 2 * p; ii++) + { + newData[pos2 + ii + p * 4] = map->map[pos1 + ii + p] / valOfNorm; // 归一化 + }/*for(ii = 0; ii < 2 * p; ii++)*/ + valOfNorm = sqrtf( // 计算当前像素的范数 partOfNorm[(i )*(sizeX + 2) + (j )] + partOfNorm[(i )*(sizeX + 2) + (j + 1)] + partOfNorm[(i + 1)*(sizeX + 2) + (j )] + @@ -411,102 +423,159 @@ int normalizeAndTruncate(CvLSVMFeatureMapCaskade *map, const float alfa) // RESULT // Error status */ +// 对输入的CvLSVMFeatureMapCaskade对象进行PCA特征映射处理 int PCAFeatureMaps(CvLSVMFeatureMapCaskade *map) -{ - int i,j, ii, jj, k; - int sizeX, sizeY, p, pp, xp, yp, pos1, pos2; +{ + int i, j, ii, jj, k; + int sizeX, sizeY, p, pp, xp, yp, pos1, pos2; float * newData; float val; float nx, ny; - + + // 获取特征映射的宽、高和特征数量 sizeX = map->sizeX; sizeY = map->sizeY; p = map->numFeatures; + + // 定义新的特征数量pp和xp、yp等参数 pp = NUM_SECTOR * 3 + 4; yp = 4; xp = NUM_SECTOR; + // 计算nx和ny两个归一化因子 nx = 1.0f / sqrtf((float)(xp * 2)); - ny = 1.0f / sqrtf((float)(yp )); + ny = 1.0f / sqrtf((float)(yp)); + // 为新的特征映射数据分配内存 newData = (float *)malloc (sizeof(float) * (sizeX * sizeY * pp)); + if (newData == NULL) { + // 如果内存分配失败，应返回错误代码 + // 这里为了简洁省略了错误处理 + return LATENT_SVM_MEM_ERROR; // 假设的错误码 + } + // 遍历特征映射的每一个位置 for(i = 0; i < sizeY; i++) { for(j = 0; j < sizeX; j++) { + // 计算原始数据在当前位置的索引pos1 pos1 = ((i)*sizeX + j)*p; + // 计算新数据在当前位置的索引pos2 pos2 = ((i)*sizeX + j)*pp; - k = 0; + + k = 0; // 初始化k用于索引新数据的位置 + + // 计算第一个部分的新特征 for(jj = 0; jj < xp * 2; jj++) { - val = 0; + val = 0; // 初始化累加值 + // 遍历yp个原始特征值进行累加 for(ii = 0; ii < yp; ii++) { val += map->map[pos1 + yp * xp + ii * xp * 2 + jj]; - }/*for(ii = 0; ii < yp; ii++)*/ + } + // 将累加值乘以ny并存储到新数据中 newData[pos2 + k] = val * ny; k++; - }/*for(jj = 0; jj < xp * 2; jj++)*/ + } + + // 计算第二个部分的新特征 for(jj = 0; jj < xp; jj++) { - val = 0; + val = 0; // 初始化累加值 + // 遍历yp个原始特征值进行累加 for(ii = 0; ii < yp; ii++) { val += map->map[pos1 + ii * xp + jj]; - }/*for(ii = 0; ii < yp; ii++)*/ + } + // 将累加值乘以ny并存储到新数据中 newData[pos2 + k] = val * ny; k++; - }/*for(jj = 0; jj < xp; jj++)*/ + } + + // 计算第三个部分的新特征 for(ii = 0; ii < yp; ii++) { - val = 0; + val = 0; // 初始化累加值 + // 遍历2*xp个原始特征值进行累加 for(jj = 0; jj < 2 * xp; jj++) { val += map->map[pos1 + yp * xp + ii * xp * 2 + jj]; - }/*for(jj = 0; jj < xp; jj++)*/ + } + // 将累加值乘以nx并存储到新数据中 newData[pos2 + k] = val * nx; k++; - } /*for(ii = 0; ii < yp; ii++)*/ - }/*for(j = 0; j < sizeX; j++)*/ - }/*for(i = 0; i < sizeY; i++)*/ -//swop data + } + } + } + + // 交换数据，这里看起来没有实际的交换操作，可能是对数据的后续处理或注释有误 + // 更新特征数量 map->numFeatures = pp; + // 释放原始特征映射数据占用的内存 free (map->map); + // 将新的特征映射数据赋值给map的map成员 map->map = newData; + // 返回成功码 return LATENT_SVM_OK; } - -//modified from "lsvmc_routine.cpp" -//ֱڷͷCvLSVMFeatureMapCaskadeṹڴ档 - -int allocFeatureMapObject(CvLSVMFeatureMapCaskade **obj, const int sizeX, +// 分配一个CvLSVMFeatureMapCaskade对象并初始化其成员 +int allocFeatureMapObject(CvLSVMFeatureMapCaskade **obj, const int sizeX, const int sizeY, const int numFeatures) { int i; + + // 使用malloc为CvLSVMFeatureMapCaskade对象分配内存 (*obj) = (CvLSVMFeatureMapCaskade *)malloc(sizeof(CvLSVMFeatureMapCaskade)); + if (!(*obj)) { // 如果分配失败，应该返回一个错误代码，但这里假设它总是成功的 + // 实际应用中应该添加错误处理 + } + + // 初始化对象的成员变量 (*obj)->sizeX = sizeX; (*obj)->sizeY = sizeY; (*obj)->numFeatures = numFeatures; - (*obj)->map = (float *) malloc(sizeof (float) * + + // 使用malloc为map成员分配内存，它用于存储特征映射数据 + (*obj)->map = (float *) malloc(sizeof (float) * (sizeX * sizeY * numFeatures)); + if (!(*obj)->map) { // 如果分配失败，应该返回一个错误代码，但这里假设它总是成功的 + // 实际应用中应该添加错误处理，并释放已经为对象分配的内存 + free(*obj); + *obj = NULL; + return LATENT_SVM_MEM_ERROR; // 假设这是一个内存分配错误的返回码 + } + + // 将map中的所有元素初始化为0.0f for(i = 0; i < sizeX * sizeY * numFeatures; i++) { (*obj)->map[i] = 0.0f; } + + // 返回成功码 return LATENT_SVM_OK; } +// 释放一个CvLSVMFeatureMapCaskade对象占用的内存 int freeFeatureMapObject (CvLSVMFeatureMapCaskade **obj) { + // 如果obj为空，返回内存为空的错误码 if(*obj == NULL) return LATENT_SVM_MEM_NULL; + + // 释放map成员占用的内存 free((*obj)->map); + // 释放对象本身占用的内存 free(*obj); + + // 将指针设置为NULL，避免野指针 (*obj) = NULL; + + // 返回成功码 return LATENT_SVM_OK; } diff --git a/src/kcf.cpp b/src/kcf.cpp new file mode 100644 index 0000000..01b89f7 --- /dev/null +++ b/src/kcf.cpp @@ -0,0 +1,363 @@ +// Track Object---advanced by Wangmingqiang ----------------------------------------- +// 2024.6.01 at Hunan Changsha. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "kcftracker.hpp" + +#include "prometheus_gimbal_control/VisionDiff.h" +#include "gimbal_track/WindowPosition.h" + +using namespace std; +using namespace cv; + +#define MARKER_SIZE 0.18 +#define F1 300 +#define F2 300 +#define C1 320 +#define C2 240 + +static const std::string RGB_WINDOW = "RGB Image window"; + +//! Camera related parameters. +int frameWidth_; +int frameHeight_; + +float get_ros_time(ros::Time begin); //获取ros当前时间 + +std_msgs::Header imageHeader_; +cv::Mat camImageCopy_; +boost::shared_mutex mutexImageCallback_; +bool imageStatus_ = false; +boost::shared_mutex mutexImageStatus_; + +/*在接收到ROS图像话题消息时被调用。这个函数用于处理从摄像头或其他图像源接收到的图像数据，并将其转换为OpenCV格式。*/ + +// 图像话题回调函数，用于处理接收到的图像消息 +void cameraCallback(const sensor_msgs::ImageConstPtr &msg) +{ + ROS_DEBUG("[EllipseDetector] USB image received."); // 打印调试信息，表示收到了图像消息 + + cv_bridge::CvImagePtr cam_image; // 定义一个OpenCV图像指针 + + // 尝试将ROS图像消息转换为OpenCV图像 + try + { + cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); // 转换为BGR8格式的OpenCV图像 + imageHeader_ = msg->header; // 保存图像消息的头部信息 + } + catch (cv_bridge::Exception &e) // 捕获cv_bridge的异常 + { + ROS_ERROR("cv_bridge exception: %s", e.what()); // 打印错误信息 + return; // 结束函数调用 + } + + // 如果图像转换成功 + if (cam_image) + { + // 使用互斥锁来保护图像数据的复制过程 + { + boost::unique_lock lockImageCallback(mutexImageCallback_); + camImageCopy_ = cam_image->image.clone(); // 克隆图像到全局变量 + } + // 使用互斥锁来更新图像状态 + { + boost::unique_lock lockImageStatus(mutexImageStatus_); + imageStatus_ = true; // 设置图像状态为已接收 + } + frameWidth_ = cam_image->image.size().width; // 保存图像宽度 + frameHeight_ = cam_image->image.size().height; // 保存图像高度 + } + return; // 结束函数调用 +} + + +// 用此函数查看是否收到图像话题 +bool getImageStatus(void) +{ + boost::shared_lock lock(mutexImageStatus_); + return imageStatus_; +} + +//! ROS subscriber and publisher. +image_transport::Subscriber imageSubscriber_; +image_transport::Publisher image_vision_pub; +ros::Publisher pose_pub; + +cv::Rect selectRect; +cv::Point origin; +cv::Rect result; + +bool select_flag = false; +bool bRenewROI = false; // the flag to enable the implementation of KCF algorithm for the new chosen ROI +bool bBeginKCF = false; +int g_control_gimbal = 1; + +float get_ros_time(ros::Time begin) +{ + ros::Time time_now = ros::Time::now(); + float currTimeSec = time_now.sec - begin.sec; + float currTimenSec = time_now.nsec / 1e9 - begin.nsec / 1e9; + return (currTimeSec + currTimenSec); +} + +void bboxDrawCb(const gimbal_track::WindowPosition::ConstPtr &msg) +{ + if (msg->mode != 0) + { + selectRect.x = msg->origin_x; + selectRect.y = msg->origin_y; + selectRect.width = msg->width; + selectRect.height = msg->height; + selectRect &= cv::Rect(0, 0, frameWidth_, frameHeight_); + if (selectRect.width * selectRect.height > 64) + { + bRenewROI = true; + } + g_control_gimbal = 1; + } + else + { + g_control_gimbal = 0; + } +} + +/*在OpenCV中用于处理鼠标事件。这个函数在用户与图像窗口交互时被调用， +允许用户通过鼠标选择一个区域（ROI）来进行跟踪*/ +// 鼠标事件回调函数 +void onMouse(int event, int x, int y, int, void *) +{ + // 如果正在选择区域 + if (select_flag) + { + // 更新选择区域的坐标和大小 + selectRect.x = MIN(origin.x, x); // 选择区域的左上角x坐标是最小值 + selectRect.y = MIN(origin.y, y); // 选择区域的左上角y坐标是最小值 + selectRect.width = abs(x - origin.x); // 选择区域的宽度是鼠标移动的水平距离 + selectRect.height = abs(y - origin.y); // 选择区域的高度是鼠标移动的垂直距离 + // 限制选择区域在图像范围内 + selectRect &= cv::Rect(0, 0, frameWidth_, frameHeight_); + } + // 如果是鼠标左键按下事件 + if (event == CV_EVENT_LBUTTONDOWN) + { + bBeginKCF = false; // 标志位，表示不开始KCF跟踪 + select_flag = true; // 标志位，表示开始选择区域 + origin = cv::Point(x, y); // 记录鼠标按下的起始点 + selectRect = cv::Rect(x, y, 0, 0); // 初始化选择区域 + } + // 如果是鼠标左键释放事件 + else if (event == CV_EVENT_LBUTTONUP) + { + // 如果选择区域的面积小于64（可能是一个无效的选择） + if (selectRect.width * selectRect.height < 64) + { + // 不执行任何操作 + } + else + { + select_flag = false; // 标志位，表示结束选择区域 + bRenewROI = true; // 标志位，表示需要更新ROI + } + } +} + +/*定义了一个ROS服务回调函数gimbalSer，它用于处理一个名为/detection/gimbal_control的服务请求。 +服务的目的是控制云台（gimbal）的行为，根据请求的内容来决定是否开启或关闭云台的控制。*/ + +// 服务回调函数，用于处理云台控制服务请求 +bool gimbalSer(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp) +{ + // 如果请求的数据为真（通常表示关闭云台控制） + if (req.data) + { + g_control_gimbal = 0; //// 设置全局变量，表示关闭云台控制 + } + // 如果请求的数据为假（通常表示开启云台控制） + else if (selectRect.width * selectRect.height > 0) + { + bRenewROI = true; // 设置标志，表示需要更新跟踪的区域（ROI） + g_control_gimbal = 1; // 设置全局变量，表示开启云台控制 + } + else + { + bRenewROI = false; // 设置标志，表示不需要更新跟踪的区域 + bBeginKCF = false; // 设置标志，表示不需要开始KCF跟踪 + } + // 设置响应的成功标志为真 + resp.success = true; + // 根据请求的数据设置响应的消息 + resp.message = req.data ? "Gimbal Control Close" : "Gimbal Control Open"; + // 返回真，表示服务处理成功 + return true; +} + +bool HOG = true; +bool FIXEDWINDOW = false; +bool MULTISCALE = true; +bool SILENT = true; +bool LAB = false; + +// Create KCFTracker object +KCFTracker tracker(HOG, FIXEDWINDOW, MULTISCALE, LAB); + +int main(int argc, char **argv) +{ + + ros::init(argc, argv, "tracker_ros"); // // 初始化ROS节点 + ros::NodeHandle nh("~"); // 创建节点句柄 + image_transport::ImageTransport it(nh); // 图像传输对象 + ros::Rate loop_rate(30); // 定义循环频率 + bool auto_zoom, show_ui; + float max_size, min_size; + nh.param("auto_zoom", auto_zoom, false); // 从参数服务器获取参数 + nh.param("show_ui", show_ui, true); + nh.param("max_size", max_size, 0.0); + nh.param("min_size", min_size, 0.0); + std::cout << "auto_zoom: " << auto_zoom << " " + << "max_size: " << max_size << " " + << "min_size: " << min_size << std::endl; + + // 接收图像的话题 + imageSubscriber_ = it.subscribe("/gimbal/image_raw", 1, cameraCallback); + // 发送绘制图像 + image_vision_pub = it.advertise("/detection/image", 1); + + // diff + ros::Publisher position_diff_pub = nh.advertise("/gimbal/track", 10); + // ros::Publisher auto_zoom_pub = nh.advertise("/gimbal_server/auto_zoom", 10); + ros::Subscriber sub_bbox_draw = nh.subscribe("/detection/bbox_draw", 10, bboxDrawCb); + ros::ServiceServer server = nh.advertiseService("/detection/gimbal_control", gimbalSer); + + sensor_msgs::ImagePtr msg_ellipse; + + const auto wait_duration = std::chrono::milliseconds(2000); + if (show_ui) + { + cv::namedWindow(RGB_WINDOW); + cv::setMouseCallback(RGB_WINDOW, onMouse, 0); + } + + float cur_time; + float last_time; + float last_error_x, last_error_y; + float dt; + + prometheus_gimbal_control::VisionDiff error_pixels; + ros::Time begin_time = ros::Time::now(); + +while (ros::ok()) // 循环，只要ROS节点正常运行就继续执行 +{ + // 获取当前ROS时间与开始时间的差值 + cur_time = get_ros_time(begin_time); + dt = (cur_time - last_time); // 计算时间差 + if (dt > 1.0 || dt < 0.0) // 如果时间差大于1秒或小于0秒，则设置为0.05秒 + { + dt = 0.05; + } + // 等待直到接收到图像 + while (!getImageStatus()) + { + printf("Waiting for image.\n"); // 打印等待图像的消息 + std::this_thread::sleep_for(wait_duration); // 休眠一段时间 + ros::spinOnce(); // 更新ROS事件循环 + } + + Mat frame; // 定义一个OpenCV图像矩阵 + { + boost::unique_lock lockImageCallback(mutexImageCallback_); + frame = camImageCopy_.clone(); // 克隆图像到局部变量 + } + if (bRenewROI) // 如果需要更新ROI + { + tracker.init(selectRect, frame); // 初始化跟踪器 + cv::rectangle(frame, selectRect, cv::Scalar(255, 0, 0), 2, 8, 0); // 在图像上绘制ROI + bRenewROI = false; // 重置更新ROI的标志 + bBeginKCF = true; // 开始KCF跟踪 + } + else if (bBeginKCF) // 如果已经开始KCF跟踪 + { + result = tracker.update(frame); // 更新跟踪结果 + error_pixels.detect = 1; // 设置检测到的标志 + + error_pixels.objectX = result.x; // 对象在图像中的位置 + error_pixels.objectY = result.y; + error_pixels.objectWidth = result.width; + error_pixels.objectHeight = result.height; + + error_pixels.frameWidth = frameWidth_; // 图像的宽度 + error_pixels.frameHeight = frameHeight_; // 图像的高度 + + error_pixels.currSize = (float)result.width * (float)result.height / (frameHeight_ * frameWidth_); // 当前对象的大小 + error_pixels.maxSize = (float)selectRect.width * (float)selectRect.height / (frameHeight_ * frameWidth_); // 最大对象大小 + + cv::rectangle(frame, result, cv::Scalar(255, 0, 0), 2, 8, 0); // 在图像上绘制跟踪结果 + } + else // 如果未开始KCF跟踪 + { + error_pixels.detect = 0; // 设置未检测到的标志 + } + error_pixels.kp = 0.2; // 比例常数 + error_pixels.ki = 0.0001; // 积分常数 + error_pixels.kd = 0.003; // 微分常数 + if (max_size != 0 && min_size != 0 && auto_zoom) // 如果自动缩放被启用 + { + error_pixels.maxSize = max_size; // 设置最大对象大小 + error_pixels.minSize = min_size; // 设置最小对象大小 + } + error_pixels.autoZoom = auto_zoom; // 设置自动缩放标志 + error_pixels.trackIgnoreError = 35; // 设置跟踪忽略误差 + if (g_control_gimbal == 0) // 如果云台控制关闭 + { + error_pixels.detect = 0; // 设置检测到的标志为0 + } + position_diff_pub.publish(error_pixels); // 发布位置差异信息 + // auto_zoom_pub.publish(error_pixels); + + float left_point = frame.cols / 2 - 20; + float right_point = frame.cols / 2 + 20; + float up_point = frame.rows / 2 + 20; + float down_point = frame.rows / 2 - 20; + // draw + line(frame, Point(left_point, frame.rows / 2), Point(right_point, frame.rows / 2), Scalar(0, 255, 0), 1, 8); + line(frame, Point(frame.cols / 2, down_point), Point(frame.cols / 2, up_point), Scalar(0, 255, 0), 1, 8); + putText(frame, "x:", Point(50, 60), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 3, 8); + putText(frame, "y:", Point(50, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 3, 8); + + // draw + char s[20] = ""; + sprintf(s, "%.2f", float(result.x + result.width / 2 - frame.cols / 2)); + putText(frame, s, Point(100, 60), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 2, 8); + sprintf(s, "%.2f", float(result.y + result.height / 2 - frame.rows / 2)); + putText(frame, s, Point(100, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 2, 8); + + if (show_ui) + { + imshow(RGB_WINDOW, frame); + waitKey(20); + } + + image_vision_pub.publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg()); + ros::spinOnce(); + loop_rate.sleep(); + } +} diff --git a/src/kcftracker.cpp b/src/kcftracker.cpp index 860b320..d45e32f 100644 --- a/src/kcftracker.cpp +++ b/src/kcftracker.cpp @@ -1,84 +1,4 @@ -/* -Tracker based on Kernelized Correlation Filter (KCF) [1] and Circulant Structure with Kernels (CSK) [2]. -CSK is implemented by using raw gray level features, since it is a single-channel filter. -KCF is implemented by using HOG features (the default), since it extends CSK to multiple channels. - -[1] J. F. Henriques, R. Caseiro, P. Martins, J. Batista, -"High-Speed Tracking with Kernelized Correlation Filters", TPAMI 2015. - -[2] J. F. Henriques, R. Caseiro, P. Martins, J. Batista, -"Exploiting the Circulant Structure of Tracking-by-detection with Kernels", ECCV 2012. - -Authors: Joao Faro, Christian Bailer, Joao F. Henriques -Contacts: joaopfaro@gmail.com, Christian.Bailer@dfki.de, henriques@isr.uc.pt -Institute of Systems and Robotics - University of Coimbra / Department Augmented Vision DFKI - - -Constructor parameters, all boolean: - hog: use HOG features (default), otherwise use raw pixels - fixed_window: fix window size (default), otherwise use ROI size (slower but more accurate) - multiscale: use multi-scale tracking (default; cannot be used with fixed_window = true) - -Default values are set for all properties of the tracker depending on the above choices. -Their values can be customized further before calling init(): - interp_factor: linear interpolation factor for adaptation - sigma: gaussian kernel bandwidth - lambda: regularization - cell_size: HOG cell size - padding: area surrounding the target, relative to its size - output_sigma_factor: bandwidth of gaussian target - template_size: template size in pixels, 0 to use ROI size - scale_step: scale step for multi-scale estimation, 1 to disable it - scale_weight: to downweight detection scores of other scales for added stability - -For speed, the value (template_size/cell_size) should be a power of 2 or a product of small prime numbers. - -Inputs to init(): - image is the initial frame. - roi is a cv::Rect with the target positions in the initial frame - -Inputs to update(): - image is the current frame. - -Outputs of update(): - cv::Rect with target positions for the current frame - - -By downloading, copying, installing or using the software you agree to this license. -If you do not agree to this license, do not download, install, -copy or use the software. - - - License Agreement - For Open Source Computer Vision Library - (3-clause BSD License) - -Redistribution and use in source and binary forms, with or without modification, -are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the names of the copyright holders nor the names of the contributors - may be used to endorse or promote products derived from this software - without specific prior written permission. - -This software is provided by the copyright holders and contributors "as is" and -any express or implied warranties, including, but not limited to, the implied -warranties of merchantability and fitness for a particular purpose are disclaimed. -In no event shall copyright holders or contributors be liable for any direct, -indirect, incidental, special, exemplary, or consequential damages -(including, but not limited to, procurement of substitute goods or services; -loss of use, data, or profits; or business interruption) however caused -and on any theory of liability, whether in contract, strict liability, -or tort (including negligence or otherwise) arising in any way out of -the use of this software, even if advised of the possibility of such damage. - */ #include #ifndef _KCFTRACKER_HEADERS #include "kcftracker.hpp" @@ -88,75 +8,74 @@ the use of this software, even if advised of the possibility of such damage. #include "labdata.hpp" #endif -// Constructor +// KCFTracker类的构造函数，用于初始化KCF跟踪器的参数 KCFTracker::KCFTracker(bool hog, bool fixed_window, bool multiscale, bool lab) { + // 所有情况下参数都相同 + lambda = 0.0001; // 权重参数 + padding = 2.5; // 边界填充参数 + output_sigma_factor = 0.125; // 输出高斯函数的标准差缩放因子 - // Parameters equal in all cases - lambda = 0.0001; - padding = 2.5; - //output_sigma_factor = 0.1; - output_sigma_factor = 0.125; - - - if (hog) { // HOG + // 根据是否使用HOG特征来设置参数 + if (hog) { // 如果使用HOG特征 // VOT - interp_factor = 0.012; - sigma = 0.6; - // TPAMI - //interp_factor = 0.02; - //sigma = 0.5; - cell_size = 4; - _hogfeatures = true; + interp_factor = 0.012; // 插值因子 + sigma = 0.6; // 高斯核的标准差 + cell_size = 4; // 大小 + _hogfeatures = true; // 设置HOG特征标志为真 + + // 如果使用LAB特征 if (lab) { - interp_factor = 0.005; - sigma = 0.4; + interp_factor = 0.005; // 插值因子 + sigma = 0.4; // 高斯核的标准差 //output_sigma_factor = 0.025; - output_sigma_factor = 0.1; + output_sigma_factor = 0.1; // 输出高斯函数的标准差缩放因子 - _labfeatures = true; - _labCentroids = cv::Mat(nClusters, 3, CV_32FC1, &data); - cell_sizeQ = cell_size*cell_size; + _labfeatures = true; // 设置LAB特征标志为真 + _labCentroids = cv::Mat(nClusters, 3, CV_32FC1, &data); // 初始化LAB特征的聚类中心 + cell_sizeQ = cell_size * cell_size; // 计算细胞大小的平方 } - else{ - _labfeatures = false; + else { + _labfeatures = false; // 设置LAB特征标志为假 } } - else { // RAW - interp_factor = 0.075; - sigma = 0.2; - cell_size = 1; - _hogfeatures = false; + else { // 如果使用原始特征 + interp_factor = 0.075; // 插值因子 + sigma = 0.2; // 高斯核的标准差 + cell_size = 1; // 细胞大小 + _hogfeatures = false; // 设置HOG特征标志为假 + // 如果尝试使用LAB特征，则打印错误信息 if (lab) { printf("Lab features are only used with HOG features.\n"); - _labfeatures = false; + _labfeatures = false; // 设置LAB特征标志为假 } } - - if (multiscale) { // multiscale - template_size = 96; + // 根据是否使用多尺度来设置参数 + if (multiscale) { // 如果使用多尺度 + template_size = 96; // 模板大小 //template_size = 100; - scale_step = 1.05; - scale_weight = 0.95; - if (!fixed_window) { + scale_step = 1.05; // 缩放步长 + scale_weight = 0.95; // 缩放权重 + if (!fixed_window) { // 如果不是固定窗口 //printf("Multiscale does not support non-fixed window.\n"); - fixed_window = true; + fixed_window = true; // 设置固定窗口标志为真 } } - else if (fixed_window) { // fit correction without multiscale - template_size = 96; + else if (fixed_window) { // 如果使用固定窗口 + template_size = 96; // 模板大小 //template_size = 100; - scale_step = 1; + scale_step = 1; // 缩放步长 } - else { - template_size = 1; - scale_step = 1; + else { // 如果都不使用 + template_size = 1; // 模板大小 + scale_step = 1; // 缩放步长 } } + // Initialize tracker void KCFTracker::init(const cv::Rect &roi, cv::Mat image) { @@ -172,51 +91,51 @@ void KCFTracker::init(const cv::Rect &roi, cv::Mat image) // Update position based on the new frame cv::Rect KCFTracker::update(cv::Mat image) { - if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 1; - if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 1; - if (_roi.x >= image.cols - 1) _roi.x = image.cols - 2; - if (_roi.y >= image.rows - 1) _roi.y = image.rows - 2; - - float cx = _roi.x + _roi.width / 2.0f; - float cy = _roi.y + _roi.height / 2.0f; + if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 1; // 如果ROI的x坐标加上宽度小于0，则调整x坐标 + if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 1; // 如果ROI的y坐标加上高度小于0，则调整y坐标 + if (_roi.x >= image.cols - 1) _roi.x = image.cols - 2; // 如果ROI的x坐标加上宽度大于图像宽度，则调整x坐标 + if (_roi.y >= image.rows - 1) _roi.y = image.rows - 2; // 如果ROI的y坐标加上高度大于图像高度，则调整y坐标 + float cx = _roi.x + _roi.width / 2.0f; // 计算ROI的中心x坐标 + float cy = _roi.y + _roi.height / 2.0f; // 计算ROI的中心y坐标 - float peak_value; - cv::Point2f res = detect(_tmpl, getFeatures(image, 0, 1.0f), peak_value); + float peak_value; // 峰值值 + cv::Point2f res = detect(_tmpl, getFeatures(image, 0, 1.0f), peak_value); // 使用检测函数和模板进行检测 - if (scale_step != 1) { - // Test at a smaller _scale + if (scale_step != 1) { // 如果缩放步长不是1，则尝试在更小的缩放级别上进行检测 + // 在更小的缩放级别上进行测试 float new_peak_value; cv::Point2f new_res = detect(_tmpl, getFeatures(image, 0, 1.0f / scale_step), new_peak_value); - if (scale_weight * new_peak_value > peak_value) { + if (scale_weight * new_peak_value > peak_value) { // 如果新峰值值更大，则更新结果 res = new_res; peak_value = new_peak_value; - _scale /= scale_step; - _roi.width /= scale_step; - _roi.height /= scale_step; + _scale /= scale_step; // 更新缩放级别 + _roi.width /= scale_step; // 更新ROI的宽度 + _roi.height /= scale_step; // 更新ROI的高度 } - // Test at a bigger _scale + // 在更大的缩放级别上进行测试 new_res = detect(_tmpl, getFeatures(image, 0, scale_step), new_peak_value); - if (scale_weight * new_peak_value > peak_value) { + if (scale_weight * new_peak_value > peak_value) { // 如果新峰值值更大，则更新结果 res = new_res; peak_value = new_peak_value; - _scale *= scale_step; - _roi.width *= scale_step; - _roi.height *= scale_step; + _scale *= scale_step; // 更新缩放级别 + _roi.width *= scale_step; // 更新ROI的宽度 + _roi.height *= scale_step; // 更新ROI的高度 } } - // Adjust by cell size and _scale - _roi.x = cx - _roi.width / 2.0f + ((float) res.x * cell_size * _scale); - _roi.y = cy - _roi.height / 2.0f + ((float) res.y * cell_size * _scale); + // 根据细胞大小和缩放级别调整ROI + _roi.x = cx - _roi.width / 2.0f + ((float) res.x * cell_size * _scale); // 调整ROI的x坐标 + _roi.y = cy - _roi.height / 2.0f + ((float) res.y * cell_size * _scale); // 调整ROI的y坐标 + + if (_roi.x >= image.cols - 1) _roi.x = image.cols - 1; // 确保ROI的x坐标不超过图像宽度 + if (_roi.y >= image.rows - 1) _roi.y = image.rows - 1; // 确保ROI的y坐标不超过图像高度 + if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 2; // 确保ROI的x坐标加上宽度大于0 + if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 2; // 确保ROI的y坐标加上高度大于0 - if (_roi.x >= image.cols - 1) _roi.x = image.cols - 1; - if (_roi.y >= image.rows - 1) _roi.y = image.rows - 1; - if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 2; - if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 2; assert(_roi.width >= 0 && _roi.height >= 0); cv::Mat x = getFeatures(image, 0); @@ -261,30 +180,20 @@ cv::Point2f KCFTracker::detect(cv::Mat z, cv::Mat x, float &peak_value) return p; } -// train tracker with a single image +// 使用单个图像训练跟踪器 void KCFTracker::train(cv::Mat x, float train_interp_factor) { - using namespace FFTTools; - - cv::Mat k = gaussianCorrelation(x, x); - cv::Mat alphaf = complexDivision(_prob, (fftd(k) + lambda)); - - _tmpl = (1 - train_interp_factor) * _tmpl + (train_interp_factor) * x; - _alphaf = (1 - train_interp_factor) * _alphaf + (train_interp_factor) * alphaf; - + using namespace FFTTools; // 引入FFTTools命名空间，可能包含相关函数和变量 - /*cv::Mat kf = fftd(gaussianCorrelation(x, x)); - cv::Mat num = complexMultiplication(kf, _prob); - cv::Mat den = complexMultiplication(kf, kf + lambda); - - _tmpl = (1 - train_interp_factor) * _tmpl + (train_interp_factor) * x; - _num = (1 - train_interp_factor) * _num + (train_interp_factor) * num; - _den = (1 - train_interp_factor) * _den + (train_interp_factor) * den; + cv::Mat k = gaussianCorrelation(x, x); // 计算x和x之间的高斯相关性 + cv::Mat alphaf = complexDivision(_prob, (fftd(k) + lambda)); // 计算高斯核的傅里叶变换，并除以(核变换 + lambda) - _alphaf = complexDivision(_num, _den);*/ + _tmpl = (1 - train_interp_factor) * _tmpl + (train_interp_factor) * x; // 更新模板 + _alphaf = (1 - train_interp_factor) * _alphaf + (train_interp_factor) * alphaf; // 更新高斯核的傅里叶变换 } + // Evaluates a Gaussian kernel with bandwidth SIGMA for all relative shifts between input images X and Y, which must both be MxN. They must also be periodic (ie., pre-processed with a cosine window). cv::Mat KCFTracker::gaussianCorrelation(cv::Mat x1, cv::Mat x2) { @@ -321,27 +230,28 @@ cv::Mat KCFTracker::gaussianCorrelation(cv::Mat x1, cv::Mat x2) return k; } -// Create Gaussian Peak. Function called only in the first frame. +// 在第一帧中创建高斯峰值 cv::Mat KCFTracker::createGaussianPeak(int sizey, int sizex) { - cv::Mat_ res(sizey, sizex); + cv::Mat_ res(sizey, sizex); // 创建一个浮点数矩阵，大小为sizey x sizex - int syh = (sizey) / 2; - int sxh = (sizex) / 2; + int syh = (sizey) / 2; // 计算y方向的高斯峰值中心 + int sxh = (sizex) / 2; // 计算x方向的高斯峰值中心 - float output_sigma = std::sqrt((float) sizex * sizey) / padding * output_sigma_factor; - float mult = -0.5 / (output_sigma * output_sigma); + float output_sigma = std::sqrt((float) sizex * sizey) / padding * output_sigma_factor; // 计算输出高斯核的标准差 + float mult = -0.5 / (output_sigma * output_sigma); // 计算高斯核的缩放因子 - for (int i = 0; i < sizey; i++) - for (int j = 0; j < sizex; j++) + for (int i = 0; i < sizey; i++) // 遍历矩阵的每一行 + for (int j = 0; j < sizex; j++) // 遍历矩阵的每一列 { - int ih = i - syh; - int jh = j - sxh; - res(i, j) = std::exp(mult * (float) (ih * ih + jh * jh)); + int ih = i - syh; // 计算当前点与y方向高斯峰值中心的距离 + int jh = j - sxh; // 计算当前点与x方向高斯峰值中心的距离 + res(i, j) = std::exp(mult * (float) (ih * ih + jh * jh)); // 计算高斯函数的值 } - return FFTTools::fftd(res); + return FFTTools::fftd(res); // 返回高斯峰值的傅里叶变换 } + // Obtain sub-window from image, with replication-padding and extract features cv::Mat KCFTracker::getFeatures(const cv::Mat & image, bool inithann, float scale_adjust) { @@ -486,22 +396,25 @@ cv::Mat KCFTracker::getFeatures(const cv::Mat & image, bool inithann, float scal return FeaturesMap; } -// Initialize Hanning window. Function called only in the first frame. +// 初始化汉明窗口。该函数仅在第一帧调用。 void KCFTracker::createHanningMats() -{ +{ cv::Mat hann1t = cv::Mat(cv::Size(size_patch[1],1), CV_32F, cv::Scalar(0)); - cv::Mat hann2t = cv::Mat(cv::Size(1,size_patch[0]), CV_32F, cv::Scalar(0)); + cv::Mat hann2t = cv::Mat(cv::Size(1,size_patch[0]), CV_32F, cv::Scalar(0)); + // 创建汉明窗口的一维版本 for (int i = 0; i < hann1t.cols; i++) - hann1t.at (0, i) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann1t.cols - 1))); + hann1t.at(0, i) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann1t.cols - 1))); for (int i = 0; i < hann2t.rows; i++) - hann2t.at (i, 0) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann2t.rows - 1))); + hann2t.at(i, 0) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann2t.rows - 1))); + // 将一维汉明窗口组合成二维汉明窗口 cv::Mat hann2d = hann2t * hann1t; - // HOG features + + // HOG特征 if (_hogfeatures) { - cv::Mat hann1d = hann2d.reshape(1,1); // Procedure do deal with cv::Mat multichannel bug - + cv::Mat hann1d = hann2d.reshape(1,1); // 解决cv::Mat多通道bug的步骤 + hann = cv::Mat(cv::Size(size_patch[0]*size_patch[1], size_patch[2]), CV_32F, cv::Scalar(0)); for (int i = 0; i < size_patch[2]; i++) { for (int j = 0; j Date: Fri, 7 Jun 2024 21:14:57 +0800 Subject: [PATCH 4/9] load --- src/GPS/CMakeLists.txt | 26 - src/GPS/GPSManager.h | 42 - src/GPS/GPSPositionMessage.h | 26 - src/GPS/GPSProvider.h | 84 -- src/GPS/RTCM/RTCMMavlink.cc | 76 -- src/GPS/RTCM/RTCMMavlink.h | 33 - src/GPS/definitions.h | 63 -- src/GPS/satellite_info.h | 24 - src/GPS/vehicle_gps_position.h | 39 - src/{GPS/GPSManager.cc => GPSManager.cpp} | 0 src/{GPS/GPSProvider.cc => GPSProvider.cpp} | 0 src/JsonHelper.cpp | 520 +++++++++++++ src/ViewLink.h | 822 ++++++++++++++++++++ src/gimbal_base.hpp | 454 +++++++++++ src/gimbal_server.cpp | 407 ++++++++++ src/serial.cpp | 293 +++++++ 16 files changed, 2496 insertions(+), 413 deletions(-) delete mode 100644 src/GPS/CMakeLists.txt delete mode 100644 src/GPS/GPSManager.h delete mode 100644 src/GPS/GPSPositionMessage.h delete mode 100644 src/GPS/GPSProvider.h delete mode 100644 src/GPS/RTCM/RTCMMavlink.cc delete mode 100644 src/GPS/RTCM/RTCMMavlink.h delete mode 100644 src/GPS/definitions.h delete mode 100644 src/GPS/satellite_info.h delete mode 100644 src/GPS/vehicle_gps_position.h rename src/{GPS/GPSManager.cc => GPSManager.cpp} (100%) rename src/{GPS/GPSProvider.cc => GPSProvider.cpp} (100%) create mode 100644 src/JsonHelper.cpp create mode 100644 src/ViewLink.h create mode 100644 src/gimbal_base.hpp create mode 100644 src/gimbal_server.cpp create mode 100644 src/serial.cpp diff --git a/src/GPS/CMakeLists.txt b/src/GPS/CMakeLists.txt deleted file mode 100644 index d969215..0000000 --- a/src/GPS/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ - - -add_library(gps - Drivers/src/ashtech.cpp - Drivers/src/gps_helper.cpp - Drivers/src/mtk.cpp - Drivers/src/rtcm.cpp - Drivers/src/sbf.cpp - Drivers/src/ubx.cpp - GPSManager.cc - GPSProvider.cc - RTCM/RTCMMavlink.cc -) - -target_link_libraries(gps - Qt6::Core - Qt6::Location - Qt6::SerialPort - Qt6::Svg - Qt6::TextToSpeech - - qgc -) - -target_include_directories(gps INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) - diff --git a/src/GPS/GPSManager.h b/src/GPS/GPSManager.h deleted file mode 100644 index dffa72e..0000000 --- a/src/GPS/GPSManager.h +++ /dev/null @@ -1,42 +0,0 @@ - - -#pragma once - -#include "GPSProvider.h" -#include "RTCM/RTCMMavlink.h" -#include - -#include -#include - -/** - ** class GPSManager - * handles a GPS provider and RTK - */ -class GPSManager : public QGCTool -{ - Q_OBJECT -public: - GPSManager(QGCApplication* app, QGCToolbox* toolbox); - ~GPSManager(); - - void connectGPS (const QString& device, const QString& gps_type); - void disconnectGPS (void); - bool connected (void) const { return _gpsProvider && _gpsProvider->isRunning(); } - -signals: - void onConnect(); - void onDisconnect(); - void surveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active); - void satelliteUpdate(int numSats); - -private slots: - void GPSPositionUpdate(GPSPositionMessage msg); - void GPSSatelliteUpdate(GPSSatelliteMessage msg); - -private: - GPSProvider* _gpsProvider = nullptr; - RTCMMavlink* _rtcmMavlink = nullptr; - - std::atomic_bool _requestGpsStop; ///< signals the thread to quit -}; diff --git a/src/GPS/GPSPositionMessage.h b/src/GPS/GPSPositionMessage.h deleted file mode 100644 index 3f5ba29..0000000 --- a/src/GPS/GPSPositionMessage.h +++ /dev/null @@ -1,26 +0,0 @@ - - - -#pragma once - -#include "vehicle_gps_position.h" -#include "satellite_info.h" -#include - -/** - ** struct GPSPositionMessage - * wrapper that can be used for Qt signal/slots - */ -struct GPSPositionMessage -{ - sensor_gps_s position_data; -}; - -Q_DECLARE_METATYPE(GPSPositionMessage); - - -struct GPSSatelliteMessage -{ - satellite_info_s satellite_data; -}; -Q_DECLARE_METATYPE(GPSSatelliteMessage); diff --git a/src/GPS/GPSProvider.h b/src/GPS/GPSProvider.h deleted file mode 100644 index a901f8b..0000000 --- a/src/GPS/GPSProvider.h +++ /dev/null @@ -1,84 +0,0 @@ - -#pragma once - -#include -#include -#include -#include - -#include - -#include "GPSPositionMessage.h" -#include "Drivers/src/gps_helper.h" - - -/** - ** class GPSProvider - * opens a GPS device and handles the protocol - */ -class GPSProvider : public QThread -{ - Q_OBJECT -public: - - enum class GPSType { - u_blox, - trimble, - septentrio - }; - - GPSProvider(const QString& device, - GPSType type, - bool enableSatInfo, - double surveyInAccMeters, - int surveryInDurationSecs, - bool useFixedBaseLocation, - double fixedBaseLatitude, - double fixedBaseLongitude, - float fixedBaseAltitudeMeters, - float fixedBaseAccuracyMeters, - const std::atomic_bool& requestStop); - ~GPSProvider(); - - /** - * this is called by the callback method - */ - void gotRTCMData(uint8_t *data, size_t len); - -signals: - void positionUpdate(GPSPositionMessage message); - void satelliteInfoUpdate(GPSSatelliteMessage message); - void RTCMDataUpdate(QByteArray message); - void surveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active); - -protected: - void run(); - -private: - void publishGPSPosition(); - void publishGPSSatellite(); - - /** - * callback from the driver for the platform specific stuff - */ - static int callbackEntry(GPSCallbackType type, void *data1, int data2, void *user); - - int callback(GPSCallbackType type, void *data1, int data2); - - QString _device; - GPSType _type; - const std::atomic_bool& _requestStop; - double _surveyInAccMeters; - int _surveryInDurationSecs; - bool _useFixedBaseLoction; - double _fixedBaseLatitude; - double _fixedBaseLongitude; - float _fixedBaseAltitudeMeters; - float _fixedBaseAccuracyMeters; - GPSHelper::GPSConfig _gpsConfig{}; - - struct sensor_gps_s _reportGpsPos; - struct satellite_info_s *_pReportSatInfo = nullptr; - - QSerialPort *_serial = nullptr; -}; diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc deleted file mode 100644 index 19dedd7..0000000 --- a/src/GPS/RTCM/RTCMMavlink.cc +++ /dev/null @@ -1,76 +0,0 @@ - - - -#include "RTCMMavlink.h" - -#include "MultiVehicleManager.h" -#include "Vehicle.h" - -#include - -RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox) - : _toolbox(toolbox) -{ - _bandwidthTimer.start(); -} - -void RTCMMavlink::RTCMDataUpdate(QByteArray message) -{ - /* statistics */ - _bandwidthByteCounter += message.size(); - qint64 elapsed = _bandwidthTimer.elapsed(); - if (elapsed > 1000) { - printf("RTCM bandwidth: %.2f kB/s\n", (float) _bandwidthByteCounter / elapsed * 1000.f / 1024.f); - _bandwidthTimer.restart(); - _bandwidthByteCounter = 0; - } - - const qsizetype maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN; - mavlink_gps_rtcm_data_t mavlinkRtcmData; - memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_rtcm_data_t)); - - if (message.size() < maxMessageLength) { - mavlinkRtcmData.len = message.size(); - mavlinkRtcmData.flags = (_sequenceId & 0x1F) << 3; - memcpy(&mavlinkRtcmData.data, message.data(), message.size()); - sendMessageToVehicle(mavlinkRtcmData); - } else { - // We need to fragment - - uint8_t fragmentId = 0; // Fragment id indicates the fragment within a set - int start = 0; - while (start < message.size()) { - int length = std::min(message.size() - start, maxMessageLength); - mavlinkRtcmData.flags = 1; // LSB set indicates message is fragmented - mavlinkRtcmData.flags |= fragmentId++ << 1; // Next 2 bits are fragment id - mavlinkRtcmData.flags |= (_sequenceId & 0x1F) << 3; // Next 5 bits are sequence id - mavlinkRtcmData.len = length; - memcpy(&mavlinkRtcmData.data, message.data() + start, length); - sendMessageToVehicle(mavlinkRtcmData); - start += length; - } - } - ++_sequenceId; -} - -void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg) -{ - QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles(); - MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol(); - for (int i = 0; i < vehicles.count(); i++) { - Vehicle* vehicle = qobject_cast(vehicles[i]); - WeakLinkInterfacePtr weakLink = vehicle->vehicleLinkManager()->primaryLink(); - - if (!weakLink.expired()) { - mavlink_message_t message; - SharedLinkInterfacePtr sharedLink = weakLink.lock(); - - mavlink_msg_gps_rtcm_data_encode_chan(mavlinkProtocol->getSystemId(), - mavlinkProtocol->getComponentId(), - sharedLink->mavlinkChannel(), - &message, - &msg); - vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message); - } - } -} diff --git a/src/GPS/RTCM/RTCMMavlink.h b/src/GPS/RTCM/RTCMMavlink.h deleted file mode 100644 index a5d4bbc..0000000 --- a/src/GPS/RTCM/RTCMMavlink.h +++ /dev/null @@ -1,33 +0,0 @@ - - - -#pragma once - -#include -#include - -#include "QGCToolbox.h" -#include "MAVLinkProtocol.h" - -/** - ** class RTCMMavlink - * Receives RTCM updates and sends them via MAVLINK to the device - */ -class RTCMMavlink : public QObject -{ - Q_OBJECT -public: - RTCMMavlink(QGCToolbox& toolbox); - //TODO: API to select device(s)? - -public slots: - void RTCMDataUpdate(QByteArray message); - -private: - void sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg); - - QGCToolbox& _toolbox; - QElapsedTimer _bandwidthTimer; - int _bandwidthByteCounter = 0; - uint8_t _sequenceId = 0; -}; diff --git a/src/GPS/definitions.h b/src/GPS/definitions.h deleted file mode 100644 index b4377d3..0000000 --- a/src/GPS/definitions.h +++ /dev/null @@ -1,63 +0,0 @@ - - -/** - * @file definitions.h - * common platform-specific definitions & abstractions for gps - * @author Beat Küng - */ - -#pragma once - -#include - -#define GPS_READ_BUFFER_SIZE 1024 - -#define GPS_INFO(...) qInfo(__VA_ARGS__) -#define GPS_WARN(...) qWarning(__VA_ARGS__) -#define GPS_ERR(...) qCritical(__VA_ARGS__) - -#include "vehicle_gps_position.h" -#include "satellite_info.h" - -#define M_DEG_TO_RAD (M_PI / 180.0) -#define M_RAD_TO_DEG (180.0 / M_PI) -#define M_DEG_TO_RAD_F 0.01745329251994f -#define M_RAD_TO_DEG_F 57.2957795130823f - -#include - -class Sleeper : public QThread -{ -public: - static void usleep(unsigned long usecs) { QThread::usleep(usecs); } -}; - -static inline void gps_usleep(unsigned long usecs) { - Sleeper::usleep(usecs); -} - -typedef uint64_t gps_abstime; - -#include -/** - * Get the current time in us. Function signature: - * uint64_t hrt_absolute_time() - */ -static inline gps_abstime gps_absolute_time() { - //FIXME: is there something with microsecond accuracy? - return QDateTime::currentMSecsSinceEpoch() * 1000; -} - -//timespec is UNIX-specific -#ifdef _WIN32 -#if _MSC_VER < 1900 -struct timespec -{ - time_t tv_sec; - long tv_nsec; -}; -#else -#include -#endif -#endif - diff --git a/src/GPS/satellite_info.h b/src/GPS/satellite_info.h deleted file mode 100644 index e2d1450..0000000 --- a/src/GPS/satellite_info.h +++ /dev/null @@ -1,24 +0,0 @@ - - -#pragma once -#include - -/* - * This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/satellite_info.msg - * and was manually copied here. - */ - -struct satellite_info_s { - uint64_t timestamp; - uint8_t count; - uint8_t svid[20]; - uint8_t used[20]; - uint8_t elevation[20]; - uint8_t azimuth[20]; - uint8_t snr[20]; - uint8_t prn[20]; -#ifdef __cplusplus - static const uint8_t SAT_INFO_MAX_SATELLITES = 20; - -#endif -}; diff --git a/src/GPS/vehicle_gps_position.h b/src/GPS/vehicle_gps_position.h deleted file mode 100644 index 3e2b589..0000000 --- a/src/GPS/vehicle_gps_position.h +++ /dev/null @@ -1,39 +0,0 @@ - - - -#pragma once -#include - -/* - * This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/vehicle_gps_position.msg - * and was manually copied here. - */ - -struct sensor_gps_s { - uint64_t timestamp; - uint64_t time_utc_usec; - int32_t lat; - int32_t lon; - int32_t alt; - int32_t alt_ellipsoid; - uint16_t automatic_gain_control; - uint8_t jamming_state; - float s_variance_m_s; - float c_variance_rad; - float eph; - float epv; - float hdop; - float vdop; - int32_t noise_per_ms; - int32_t jamming_indicator; - float vel_m_s; - float vel_n_m_s; - float vel_e_m_s; - float vel_d_m_s; - float cog_rad; - int32_t timestamp_time_relative; - float heading; - uint8_t fix_type; - bool vel_ned_valid; - uint8_t satellites_used; -}; diff --git a/src/GPS/GPSManager.cc b/src/GPSManager.cpp similarity index 100% rename from src/GPS/GPSManager.cc rename to src/GPSManager.cpp diff --git a/src/GPS/GPSProvider.cc b/src/GPSProvider.cpp similarity index 100% rename from src/GPS/GPSProvider.cc rename to src/GPSProvider.cpp diff --git a/src/JsonHelper.cpp b/src/JsonHelper.cpp new file mode 100644 index 0000000..e5aa783 --- /dev/null +++ b/src/JsonHelper.cpp @@ -0,0 +1,520 @@ +/**************************************************************************** + *˴QGCҪ + ****************************************************************************/ + +#include "JsonHelper.h" +#include "QGCQGeoCoordinate.h" +#include "QmlObjectListModel.h" +#include "MissionCommandList.h" +#include "FactMetaData.h" +#include "QGCApplication.h" + +#include +#include +#include +#include +#include +#include +#include + +const char* JsonHelper::jsonVersionKey = "version"; +const char* JsonHelper::jsonGroundStationKey = "groundStation"; +const char* JsonHelper::jsonGroundStationValue = "QGroundControl"; +const char* JsonHelper::jsonFileTypeKey = "fileType"; +const char* JsonHelper::_translateKeysKey = "translateKeys"; +const char* JsonHelper::_arrayIDKeysKey = "_arrayIDKeys"; + +bool JsonHelper::validateRequiredKeys(const QJsonObject& jsonObject, const QStringList& keys, QString& errorString) +{ + QString missingKeys; + + foreach(const QString& key, keys) { + if (!jsonObject.contains(key)) { + if (!missingKeys.isEmpty()) { + missingKeys += QStringLiteral(", "); + } + missingKeys += key; + } + } + + if (missingKeys.length() != 0) { + errorString = QObject::tr("The following required keys are missing: %1").arg(missingKeys); + return false; + } + + return true; +} + +bool JsonHelper::_loadGeoCoordinate(const QJsonValue& jsonValue, + bool altitudeRequired, + QGeoCoordinate& coordinate, + QString& errorString, + bool geoJsonFormat) +{ + if (!jsonValue.isArray()) { + errorString = QObject::tr("value for coordinate is not array"); + return false; + } + + QJsonArray coordinateArray = jsonValue.toArray(); + int requiredCount = altitudeRequired ? 3 : 2; + if (coordinateArray.count() != requiredCount) { + errorString = QObject::tr("Coordinate array must contain %1 values").arg(requiredCount); + return false; + } + + for (const auto& jsonValue: coordinateArray) { + if (jsonValue.type() != QJsonValue::Double && jsonValue.type() != QJsonValue::Null) { + errorString = QObject::tr("Coordinate array may only contain double values, found: %1").arg(jsonValue.type()); + return false; + } + } + + if (geoJsonFormat) { + coordinate = QGeoCoordinate(coordinateArray[1].toDouble(), coordinateArray[0].toDouble()); + } else { + coordinate = QGeoCoordinate(possibleNaNJsonValue(coordinateArray[0]), possibleNaNJsonValue(coordinateArray[1])); + } + if (altitudeRequired) { + coordinate.setAltitude(possibleNaNJsonValue(coordinateArray[2])); + } + + return true; +} + +void JsonHelper::_saveGeoCoordinate(const QGeoCoordinate& coordinate, + bool writeAltitude, + QJsonValue& jsonValue, + bool geoJsonFormat) +{ + QJsonArray coordinateArray; + + if (geoJsonFormat) { + coordinateArray << coordinate.longitude() << coordinate.latitude(); + } else { + coordinateArray << coordinate.latitude() << coordinate.longitude(); + } + if (writeAltitude) { + coordinateArray << coordinate.altitude(); + } + + jsonValue = QJsonValue(coordinateArray); +} + +bool JsonHelper::loadGeoCoordinate(const QJsonValue& jsonValue, + bool altitudeRequired, + QGeoCoordinate& coordinate, + QString& errorString, + bool geoJsonFormat) +{ + return _loadGeoCoordinate(jsonValue, altitudeRequired, coordinate, errorString, geoJsonFormat); +} + +void JsonHelper::saveGeoCoordinate(const QGeoCoordinate& coordinate, + bool writeAltitude, + QJsonValue& jsonValue) +{ + _saveGeoCoordinate(coordinate, writeAltitude, jsonValue, false /* geoJsonFormat */); +} + +bool JsonHelper::loadGeoJsonCoordinate(const QJsonValue& jsonValue, + bool altitudeRequired, + QGeoCoordinate& coordinate, + QString& errorString) +{ + return _loadGeoCoordinate(jsonValue, altitudeRequired, coordinate, errorString, true /* geoJsonFormat */); +} + +void JsonHelper::saveGeoJsonCoordinate(const QGeoCoordinate& coordinate, + bool writeAltitude, + QJsonValue& jsonValue) +{ + _saveGeoCoordinate(coordinate, writeAltitude, jsonValue, true /* geoJsonFormat */); +} + +bool JsonHelper::validateKeyTypes(const QJsonObject& jsonObject, const QStringList& keys, const QList& types, QString& errorString) +{ + for (int i=0; i requiredKeys = { + { jsonFileTypeKey, QJsonValue::String, true }, + { jsonVersionKey, QJsonValue::Double, true }, + }; + if (!JsonHelper::validateKeys(jsonObject, requiredKeys, errorString)) { + return false; + } + + // Make sure file type is correct + QString fileTypeValue = jsonObject[jsonFileTypeKey].toString(); + if (fileTypeValue != expectedFileType) { + errorString = QObject::tr("Incorrect file type key expected:%1 actual:%2").arg(expectedFileType).arg(fileTypeValue); + return false; + } + + // Version check + version = jsonObject[jsonVersionKey].toInt(); + if (version < minSupportedVersion) { + errorString = QObject::tr("File version %1 is no longer supported").arg(version); + return false; + } + if (version > maxSupportedVersion) { + errorString = QObject::tr("File version %1 is newer than current supported version %2").arg(version).arg(maxSupportedVersion); + return false; + } + + return true; +} + +bool JsonHelper::validateExternalQGCJsonFile(const QJsonObject& jsonObject, + const QString& expectedFileType, + int minSupportedVersion, + int maxSupportedVersion, + int& version, + QString& errorString) +{ + // Validate required keys + QList requiredKeys = { + { jsonGroundStationKey, QJsonValue::String, true }, + }; + if (!JsonHelper::validateKeys(jsonObject, requiredKeys, errorString)) { + return false; + } + + return validateInternalQGCJsonFile(jsonObject, expectedFileType, minSupportedVersion, maxSupportedVersion, version, errorString); +} + +QStringList JsonHelper::_addDefaultLocKeys(QJsonObject& jsonObject) +{ + QString translateKeys; + QString fileType = jsonObject[jsonFileTypeKey].toString(); + if (!fileType.isEmpty()) { + if (fileType == MissionCommandList::qgcFileType) { + if (jsonObject.contains(_translateKeysKey)) { + translateKeys = jsonObject[_translateKeysKey].toString(); + } else { + translateKeys = "label,enumStrings,friendlyName,description,category"; + jsonObject[_translateKeysKey] = translateKeys; + } + if (!jsonObject.contains(_arrayIDKeysKey)) { + jsonObject[_arrayIDKeysKey] = "rawName,comment"; + } + } else if (fileType == FactMetaData::qgcFileType) { + if (jsonObject.contains(_translateKeysKey)) { + translateKeys = jsonObject[_translateKeysKey].toString(); + } else { + translateKeys = "shortDescription,longDescription,enumStrings"; + jsonObject[_translateKeysKey] = "shortDescription,longDescription,enumStrings"; + } + if (!jsonObject.contains(_arrayIDKeysKey)) { + jsonObject[_arrayIDKeysKey] = "name"; + } + } + } + return translateKeys.split(","); +} + +QJsonObject JsonHelper::_translateObject(QJsonObject& jsonObject, const QString& translateContext, const QStringList& translateKeys) +{ + for (const QString& key: jsonObject.keys()) { + if (jsonObject[key].isString()) { + QString locString = jsonObject[key].toString(); + if (translateKeys.contains(key)) { + QString disambiguation; + QString disambiguationPrefix("#loc.disambiguation#"); + + if (locString.startsWith(disambiguationPrefix)) { + locString = locString.right(locString.length() - disambiguationPrefix.length()); + int commentEndIndex = locString.indexOf("#"); + if (commentEndIndex != -1) { + disambiguation = locString.left(commentEndIndex); + locString = locString.right(locString.length() - disambiguation.length() - 1); + } + } + + QString xlatString = qgcApp()->qgcJSONTranslator().translate(translateContext.toUtf8().constData(), locString.toUtf8().constData(), disambiguation.toUtf8().constData()); + if (!xlatString.isNull()) { + jsonObject[key] = xlatString; + } + } + } else if (jsonObject[key].isArray()) { + QJsonArray childJsonArray = jsonObject[key].toArray(); + jsonObject[key] = _translateArray(childJsonArray, translateContext, translateKeys); + } else if (jsonObject[key].isObject()) { + QJsonObject childJsonObject = jsonObject[key].toObject(); + jsonObject[key] = _translateObject(childJsonObject, translateContext, translateKeys); + } + } + + return jsonObject; +} + +QJsonArray JsonHelper::_translateArray(QJsonArray& jsonArray, const QString& translateContext, const QStringList& translateKeys) +{ + for (int i=0; i& rgPoints, + QString& errorString) +{ + QVariantList rgVarPoints; + + if (!loadGeoCoordinateArray(jsonValue, altitudeRequired, rgVarPoints, errorString)) { + return false; + } + + rgPoints.clear(); + for (int i=0; i()); + } + + return true; +} + +void JsonHelper::saveGeoCoordinateArray(const QVariantList& rgVarPoints, + bool writeAltitude, + QJsonValue& jsonValue) +{ + QJsonArray rgJsonPoints; + + // Add all points to the array + for (int i=0; i(), writeAltitude, jsonPoint); + rgJsonPoints.append(jsonPoint); + } + + jsonValue = rgJsonPoints; +} + +void JsonHelper::saveGeoCoordinateArray(const QList& rgPoints, + bool writeAltitude, + QJsonValue& jsonValue) +{ + QVariantList rgVarPoints; + + for (int i=0; i& keyInfo, QString& errorString) +{ + QStringList keyList; + QList typeList; + + for (int i=0; i(i)->coordinate(); + + QJsonValue jsonValue; + JsonHelper::saveGeoCoordinate(vertex, false /* writeAltitude */, jsonValue); + polygonArray.append(jsonValue); + } +} + +double JsonHelper::possibleNaNJsonValue(const QJsonValue& value) +{ + if (value.type() == QJsonValue::Null) { + return std::numeric_limits::quiet_NaN(); + } else { + return value.toDouble(); + } +} diff --git a/src/ViewLink.h b/src/ViewLink.h new file mode 100644 index 0000000..bb0d994 --- /dev/null +++ b/src/ViewLink.h @@ -0,0 +1,822 @@ + + +/** + * @file ViewLink.h + * @brief ViewLink SDK header file + * @details This header file defines ViewLink SDK interfaces and the necessary data structures + * @author Edwin + * @version 3.0.7 + * @date 2020-06-28 + * @copyright Copyright (c) 2020 Shenzhen Viewpro Technology Co., Ltd + */ + +#ifndef __VIEW_LINK_H__ +#define __VIEW_LINK_H__ + +#if (defined _WIN32) || (defined _WIN64) // windows + #ifdef VLK_EXPORTS + #define VLK_API extern "C" __declspec(dllexport) + #else + #define VLK_API extern "C" __declspec(dllimport) + #endif + #define VLK_CALL __stdcall +#else + #define VLK_API + #define VLK_CALL +#endif + +/** @name error code + * @brief usually used as return value of interface function to indicate the operation success or not + * @{ + */ +#define VLK_ERROR_NO_ERROR 0 +#define VLK_ERROR_INVALID_PARAM -1 +/** @} error code */ + +/** @name ViewLink SDK constant + * @{ + */ +/** the max value of yaw */ +#define VLK_YAW_MAX 180.0 +/** the min value of yaw */ +#define VLK_YAW_MIN -180.0 +/** the max value of pitch */ +#define VLK_PITCH_MAX 90.0 +/** the min value of pitch */ +#define VLK_PITCH_MIN -90.0 + +/** the yaw max moving speed (20 degrees per second)*/ +#define VLK_MAX_YAW_SPEED 2000.0 +/** the pitch max moving speed (20 degrees per second)*/ +#define VLK_MAX_PITCH_SPEED 2000.0 + +/** the min zoom (or focus) speed*/ +#define VLK_MIN_ZOOM_SPEED 1 +/** the max zoom (or focus) speed*/ +#define VLK_MAX_ZOOM_SPEED 8 +/** @} ViewLink SDK constant */ + +/** @name ViewLink SDK data structures + * @{ + */ + +/** + * device connection type + */ +typedef enum _VLK_CONN_TYPE +{ + VLK_CONN_TYPE_SERIAL_PORT = 0x00, ///< serial port + VLK_CONN_TYPE_TCP = 0x01, ///< tcp + VLK_CONN_TYPE_UDP = 0x02, ///< udp, not currently supported + VLK_CONN_TYPE_BUTT +}VLK_CONN_TYPE; + +/** + * TCP connect information + */ +typedef struct _VLK_DEV_IPADDR VLK_DEV_IPADDR; +struct _VLK_DEV_IPADDR +{ + char szIPV4[16]; ///< device ipv4 address, e.g: 192.168.2.119 + int iPort; ///< tcp port, e.g: 2000 +}; + +/** + * serial port connect information + */ +typedef struct _VLK_DEV_SERIAL_PORT VLK_DEV_SERIAL_PORT; +struct _VLK_DEV_SERIAL_PORT +{ + char szSerialPortName[16]; ///< serial port name, e.g: /dev/ttyS0 on linux or COM1 on windows + int iBaudRate; ///< baudrate, e.g: 115200 +}; + +/** + * device connect parameter + */ +typedef struct _VLK_CONN_PARAM VLK_CONN_PARAM; +struct _VLK_CONN_PARAM +{ + VLK_CONN_TYPE emType; + union { + VLK_DEV_IPADDR IPAddr; + VLK_DEV_SERIAL_PORT SerialPort; + } ConnParam; +}; + +/** + * device connection status + */ +typedef enum _VLK_CONN_STATUS +{ + VLK_CONN_STATUS_CONNECTED = 0x00, ///< deprecated + VLK_CONN_STATUS_DISCONNECTED = 0x01, ///< deprecated + VLK_CONN_STATUS_TCP_CONNECTED = 0x02, ///< TCP connected + VLK_CONN_STATUS_TCP_DISCONNECTED = 0x03, ///< TCP disconnected + VLK_CONN_STATUS_SERIAL_PORT_CONNECTED = 0x04, ///< serial port connected + VLK_CONN_STATUS_SERIAL_PORT_DISCONNECTED = 0x05, ///< serial port disconnectd + VLK_CONN_STATUS_BUTT +}VLK_CONN_STATUS; + +/** + * track template size + */ +typedef enum _VLK_TRACK_TEMPLATE_SIZE +{ + VLK_TRACK_TEMPLATE_SIZE_AUTO = 0, + VLK_TRACK_TEMPLATE_SIZE_32 = 32, + VLK_TRACK_TEMPLATE_SIZE_64 = 64, + VLK_TRACK_TEMPLATE_SIZE_128 = 128, + VLK_TRACK_TEMPLATE_SIZE_BUTT +}VLK_TRACK_TEMPLATE_SIZE; + +/** + * sensor type + */ +typedef enum _VLK_SENSOR +{ + VLK_SENSOR_VISIBLE1 = 0, ///< visible light only + VLK_SENSOR_IR = 1, ///< IR only + VLK_SENSOR_VISIBLE_IR = 2, ///< visible light + IR PIP + VLK_SENSOR_IR_VISIBLE = 3, ///< IR + visible light PIP + VLK_SENSOR_VISIBLE2 = 4, ///< prime lens visible light + VLK_SENSOR_BUTT +}VLK_SENSOR; + +/** + * track mode parameter + */ +typedef struct _VLK_TRACK_MODE_PARAM VLK_TRACK_MODE_PARAM; +struct _VLK_TRACK_MODE_PARAM +{ + VLK_TRACK_TEMPLATE_SIZE emTrackTempSize; + VLK_SENSOR emTrackSensor; +}; + +/** + * Record mode + */ +typedef enum _VLK_RECORD_MODE +{ + VLK_RECORD_MODE_NONE = 0, ///< none mode, neither photo nor record mode + VLK_RECORD_MODE_PHOTO = 1, ///< photo mode, take photos only + VLK_RECORD_MODE_RECORD = 2, ///< record mode, video record only + VLK_RECORD_MODE_BUTT +}VLK_RECORD_MODE; + +/** + * Focus mode + */ +typedef enum _VLK_FOCUS_MODE +{ + VLK_FOCUS_MODE_AUTO = 0, ///< automatic focus + VLK_FOCUS_MODE_MANU = 1, ///< manual focus + VLK_FOCUS_MODE_BUTT +}VLK_FOCUS_MODE; + +/** + * Laser zoom mode + */ +typedef enum _VLK_LASER_ZOOM_MODE +{ + VLK_LASER_ZOOM_MODE_FOLLOW_EO = 0, ///< follow visible light zoom + VLK_LASER_ZOOM_MODE_MANU = 1, ///< manual control + VLK_LASER_ZOOM_MODE_BUTT +}VLK_LASER_ZOOM_MODE; + +/** + * OSD mask + */ +typedef enum _VLK_OSD_MASK +{ + VLK_OSD_MASK_ENABLE_OSD = 0x1, ///< enable OSD + VLK_OSD_MASK_CROSS = 0x2, ///< enable cross frame in center of image + VLK_OSD_MASK_PITCH_YAW = 0x4, ///< show pitch and yaw + VLK_OSD_MASK_XYSHIFT = 0x8, ///< enable xy shift + VLK_OSD_MASK_GPS = 0x10, ///< enable GPS + VLK_OSD_MASK_TIME = 0x20, ///< enable time + VLK_OSD_MASK_VL_MAG = 0x40, ///< enable VL-MAG + VLK_OSD_MASK_BIG_FONT = 0x80 ///< enable big font +}VLK_OSD_MASK; + +/** + * OSD input mask + */ +typedef enum _VLK_OSD_INPUT_MASK +{ + VLK_OSD_INPUT_MASK_PERMANENT_SAVE = 0x1, ///< save OSD configuration, still work after reboot + VLK_OSD_INPUT_MASK_TIME = 0x2, ///< enalbe input time + VLK_OSD_INPUT_MASK_GPS = 0x4, ///< enable input GPS + VLK_OSD_INPUT_MASK_MGRS = 0x8,///< enable input MGRS + VLK_OSD_INPUT_MASK_PITCH_YAW = 0x10, ///< enable input pitch and yaw + VLK_OSD_INPUT_MASK_VL_MAG = 0x20, ///< enalbe input VL-MAG + VLK_OSD_INPUT_MASK_ZOOM_TIMES_OR_FOV = 0x40, ///< display zoom times or FOV + VLK_OSD_INPUT_MASK_CHAR_BLACK_BORDER = 0x80 ///< enable character black frame +}VLK_OSD_INPUT_MASK; + +/** + * OSD configuration parameter + */ +typedef struct _VLK_OSD_PARAM VLK_OSD_PARAM; +struct _VLK_OSD_PARAM +{ + char cOSD; ///< mask of VLK_OSD_MASK, e.g: VLK_OSD_MASK_ENABLE_OSD | VLK_OSD_MASK_CROSS + char cOSDInput; ///< mask of VLK_OSD_INPUT_MASK, e.g: VLK_OSD_INPUT_MASK_TIME | VLK_OSD_INPUT_MASK_GPS +}; + +/** + * Image type + */ +typedef enum _VLK_IMAGE_TYPE +{ + VLK_IMAGE_TYPE_VISIBLE1, ///< visible light zoom lens + VLK_IMAGE_TYPE_VISIBLE2, ///< visible light prime lens + VLK_IMAGE_TYPE_IR1, ///< IR zoom lens + VLK_IMAGE_TYPE_IR2, ///< IR prime lens + VLK_IMAGE_TYPE_FUSION, ///< visible light + IR fusion + VLK_IMAGE_TYPE_BUTT +}VLK_IMAGE_TYPE; + +/** + * IR color type + */ +typedef enum _VLK_IR_COLOR +{ + VLK_IR_COLOR_WHITEHOT, + VLK_IR_COLOR_BLACKHOT, + VLK_IR_COLOR_PSEUDOHOT, + VLK_IR_COLOR_RUSTY, + VLK_IR_COLOR_BUTT +}VLK_IR_COLOR; + +/** + * 2.4G wireless control channels map + */ +typedef struct _VLK_CHANNELS_MAP VLK_CHANNELS_MAP; +struct _VLK_CHANNELS_MAP +{ + char cYW; ///< high 4 bits save yaw left channel index, low 4 bits save yaw right channel index + char cPT; ///< high 4 bits save pitch up channel index, low 4 bits save pitch down channel index + char cMO; ///< high 4 bits save adjust speed channel index, low 4 bits save recenter channel index + char cZM; ///< high 4 bits save zoom out channel index, low 4 bits save zoom in channel index + char cFC; ///< high 4 bits save focus out channel index, low 4 bits save focus in channel index + char cRP; ///< high 4 bits save take photo channel index, low 4 bits save Gimbal record channel index + char cMU; ///< high 4 bits save start track channel index, low 4 bits save stop track channel index +}; + +/** + * Device configuration + */ +typedef struct _VLK_DEV_CONFIG VLK_DEV_CONFIG; +struct _VLK_DEV_CONFIG +{ + char cTimeZone; ///< time zone, e.g: 8 (Beijing), 9 (Seoul) + char cOSDCfg; ///< OSD configuration, equivalent to cOSD in VLK_OSD_PARAM + char cMagneticVariation; ///< Magnetic Variation set https://skyvector.com/ http://www.magnetic-declination.com/ + char cOSDInput; ///< OSD input configuration, equivalent to cOSDInput in VLK_OSD_PARAM + + /** + * @brief Gimbal serial port baudrate \n + * 0: 2400 \n + * 1: 4800 \n + * 2: 9600 \n + * 3: 19200 \n + * 4: 38400 \n + * 5: 57600 \n + * 6: 115200 \n + * 7: S.BUS mode + */ + char cBaudRate; + + char cEODigitalZoom; ///< EO digital zoom, 1: on; 0: off + short sTemperatureAlarmLine;///< temperature alarm, low 8 bits save lower limit, high 8 bits save upper limit + char cTrack; ///< track status, 1: enabled, 0: disabled + + /** + * @brief laser work mode \n + * 0: stop LRF \n + * 1: 1HZ get LRF data \n + * 2: continuosly get LRF \n + * 3: one time get LRF data + */ + char cLaser; + + char cRecordDefinition; ///< record definition, 1: 4k 25fps; 2: 1080P 25fps + char cOSDGPS; ///< OSD GPS, 0: OSD GPS is UAV; 1: OSD GPS is target + + /** + * @brief s.bus/mavlink channels map set \n + * 1: 1~7 \n + * 2: 6~12 \n + * 3: 8~14 \n + * 4: custom channels \n + */ + char cSBUSChnlMap; + + VLK_CHANNELS_MAP ChnlsMap; ///< custom channels map + char cFocusHoldSet; ///< deprecated + char cCameraType; ///< deprecated + char cReserved1[5]; ///< reserved + char cRestoreIP; ///< deprecated + char cReserved2[5]; ///< reserved + char cReserved3[43]; ///< reserved + char cVersionNO[20]; ///< firmware version NO + char cDeviceID[10]; ///< device model + char cSerialNO[22]; ///< serial NO +}; + + +/** + * Device model + */ +typedef struct _VLK_DEV_MODEL VLK_DEV_MODEL; +struct _VLK_DEV_MODEL +{ + char cModelCode; ///< Gimbal model code + char szModelName[32]; ///< Gimbal model name +}; + +/** + * Tracker status + */ +typedef enum _VLK_TRACKER_STATUS +{ + VLK_TRACKER_STATUS_STOPPED, + VLK_TRACKER_STATUS_SEARCHING, + VLK_TRACKER_STATUS_TRACKING, + VLK_TRACKER_STATUS_LOST, + VLK_TRACKER_STATUS_BUTT +}VLK_TRACKER_STATUS; + +/** + * Device telemetry + */ +typedef struct _VLK_DEV_TELEMETRY VLK_DEV_TELEMETRY; +struct _VLK_DEV_TELEMETRY +{ + double dYaw; ///< Gimbal current yaw + double dPitch; ///< Gimbal current pitch + VLK_SENSOR emSensorType; ///< sensor type + VLK_TRACKER_STATUS emTrackerStatus; ///< tracker status + double dTargetLat; ///< target latitude + double dTargetLng; ///< target longitude + double dTargetAlt; ///< target altitude + double dZoomMagTimes; ///< camera magnification times + short sLaserDistance; ///< laser distance + VLK_IR_COLOR emIRColor; ///< IR color + VLK_RECORD_MODE emRecordMode; ///< record mode +}; + +/** + * Device status type \n + * device status is passed by VLK_DevStatus_CB, \n + * VLK_DEV_STATUS_TYPE will be passed to formal paramter iType, \n + * and the device status data will be passed to szBuffer, \n + * different iType matches different status data struct + */ +typedef enum _VLK_DEV_STATUS_TYPE +{ + VLK_DEV_STATUS_TYPE_MODEL, ///< matches VLK_DEV_MODEL + VLK_DEV_STATUS_TYPE_CONFIG, ///< matches VLK_DEV_CONFIG + VLK_DEV_STATUS_TYPE_TELEMETRY, ///< matches VLK_DEV_TELEMETRY + VLK_DEV_STATUS_TYPE_BUTT, +}VLK_DEV_STATUS_TYPE; +/** @} ViewLink SDK data structures */ + +/** @name ViewLink SDK callback functions + * @{ + */ +/** @brief connection status callback + * @param iConnStatus connection status, it must be one of VLK_CONN_STATUS + * @param szMessage extended string message + * @param iMsgLen extended string message length + * @return return 0 anyway + * @details VLK_ConnStatus_CB will be called by SDK once the connection status changed, + * for example, developers should check connect result in this callback function + * @see VLK_Connect + */ +typedef int (*VLK_ConnStatus_CB)(int iConnStatus, const char* szMessage, int iMsgLen, void* pUserParam); + +/** @brief device status callback + * @param iType device status type, it must be one of VLK_DEV_STATUS_TYPE + * @param szBuffer device status data, it is an address of a struct matchs iType, e.g: VLK_DEV_MODEL + * @param iBufLen device status data length, it must be equal to the size of struct matchs iType, e.g: iBufLen == sizeof(VLK_DEV_MODEL) + * @return return 0 anyway + * @details VLK_DevStatus_CB will be called by SDK once device new status is received, developers can easily get Gimbal latest status + * @see VLK_RegisterDevStatusCB + */ +typedef int (*VLK_DevStatus_CB)(int iType, const char* szBuffer, int iBufLen, void* pUserParam); +/** @} ViewLink SDK callback functions */ + + +#ifdef __cplusplus +extern "C" { +#endif + +/** @brief Get SDK version + * @return SDK version number, e.g: 3.0.6 + * @details we recommend printing the SDK version number before you use SDK, + * this is the only interface can be called before VLK_Init() + */ +VLK_API const char* VLK_CALL GetSDKVersion(); + +/** @brief Initialize SDK + * @return VLK_ERROR_NO_ERROR on success, error code otherwise + * @details it should be called somewhere at the beginning of your application, + * and it should be called just once during the lifecirle of your application, + * all interfaces are unavailbe before initialized + */ +VLK_API int VLK_CALL VLK_Init(); + +/** @brief Uninitialize SDK + * @details it should be called somewhere at the end of your application, + * and it should be called just once during the lifecirle of your application, + * once you uninitialize sdk, all interfaces are nomore available + */ +VLK_API void VLK_CALL VLK_UnInit(); + +/** @brief connect Gimbal with specific type, TCP and serial port are available + * @param pConnParam connect information, including connection type (TCP or serial port), ip address, port, serial port name, baudrate + * @param pConnStatusCB connnection status callback function, it will be called by SDK when device is connected or disconnected + * @param pUserParam user parameters, will be passed back when pConnStatusCB is called + * @return VLK_ERROR_NO_ERROR on success, error code otherwise + * @details this function is asynchronous, the connect result will be passed back by callback function, do not use return value to judge connect result. + * SDK allows only one connection, it means if you create a new connection, the previous one will be disconnected automatically + */ +VLK_API int VLK_CALL VLK_Connect(const VLK_CONN_PARAM* pConnParam, VLK_ConnStatus_CB pConnStatusCB, void* pUserParam); + +/** @brief Check if Gimbal is connected + * @return + * @retval 1 if either TCP or serial port is connected + * @retval 0 if neither TCP nor serial port is connected + */ +VLK_API int VLK_CALL VLK_IsConnected(); + +/** @brief Check if Gimbal is TCP connected + * @return + * @retval 1 if TCP is connected + * @retval 0 if TCP is disconnected + */ +VLK_API int VLK_CALL VLK_IsTCPConnected(); + +/** @brief Check if Gimbal is serial port connected + * @return + * @retval 1 if serial port is connected + * @retval 0 if serial port is disconnected + */ +VLK_API int VLK_CALL VLK_IsSerialPortConnected(); + +/** @brief Disconnect current connection no matter it is TCP, serial port or other + */ +VLK_API void VLK_CALL VLK_Disconnect(); + +/** @brief Disconnect current TCP connection + */ +VLK_API void VLK_CALL VLK_DisconnectTCP(); + +/** @brief Disconnect current serial port connection + */ +VLK_API void VLK_CALL VLK_DisconnectSerialPort(); + +/** @brief Set keep alive interval + * @param iMS keep alive interval in milliseconds range from 100 ms to 5000 ms + * @details keep alive interval determines the frequncy of querying Gimbal telemetry data, + * SDK default keep alive interval is 500 ms, no need to change it if not necessary + */ +VLK_API void VLK_CALL VLK_SetKeepAliveInterval(int iMS); + +/** @brief Register device status callback + * @param pDevStatusCB receive device status callback function + * @param pUserParam user parameter, it will be passed back when pDevStatusCB is called + * @details pDevStatusCB will be called once SDK received new Gimbal, + * specifically, the telemetry data will keep updating once device is connected, + * ignore it if you don't need it + */ +VLK_API void VLK_CALL VLK_RegisterDevStatusCB(VLK_DevStatus_CB pDevStatusCB, void* pUserParam); + +/** @brief Control Gimbal yaw and pitch + * @param sHorizontalSpeed the speed of changing yaw (0.01 degrees/s), for example, 2000 means 20 degrees per second + * @param sVeritcalSpeed the speed of changing pitch (0.01 degrees/s) + * @details considering a small angle adjustment will result in a huge visual field changing in sky view, we limit + * speed in a proper range: \n + * -VLK_MAX_YAW_SPEED <= sHorizontalSpeed <= VLK_MAX_YAW_SPEED \n + * -VLK_MAX_PITCH_SPEED <= sVeritcalSpeed <= VLK_MAX_PITCH_SPEED \n + * some example: \n + * move up: VLK_Move(0, 1000); \n + * move left: VLK_Move(-1000, 0); \n + * move right: VLK_Move(1000, 0); \n + * move down: VLK_Move(0, -1000); + */ +VLK_API void VLK_CALL VLK_Move(short sHorizontalSpeed, short sVeritcalSpeed); + +/** @brief Stop moving + * @details once you call VLK_Move, Gimbal will keep moving until it reach the yaw and pitch limitation or VLK_Stop is called + */ +VLK_API void VLK_CALL VLK_Stop(); + +/** @brief Zoom in + * @param sSpeed the speed of zoom in + * @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster + */ +VLK_API void VLK_CALL VLK_ZoomIn(short sSpeed); + +/** @brief Zoom out + * @param sSpeed the speed of zoom out + * @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster + */ +VLK_API void VLK_CALL VLK_ZoomOut(short sSpeed); + +/** @brief Stop zoom + * @details once you call VLK_ZoomIn or VLK_ZoomOut, Camera will keep zooming until it reach the limitation or VLK_StopZoom is called + */ +VLK_API void VLK_CALL VLK_StopZoom(); + +/** @brief Enable track mode + * @param Param track mode parameters + * @see VLK_TrackTargetPositionEx + * @deprecated + */ +VLK_API void VLK_CALL VLK_EnableTrackMode(const VLK_TRACK_MODE_PARAM* pParam); + +/** @brief Track target by it's position + * @param iX target position, number of pixels from top-left corner in horizontal direction + * @param iY taget position, number of pixels from top-left corner in vertical direction + * @param iVideoWidth video image width, e.g: 1920, 1280 + * @param iVideoHeight video image height, e.g: 1080, 720 + * @deprecated + * @see VLK_TrackTargetPositionEx + */ +VLK_API void VLK_CALL VLK_TrackTargetPosition(int iX, int iY, int iVideoWidth, int iVideoHeight); + +/** @brief Track target by it's position + * @param pParam track mode parameters, + * @param iX target position, number of pixels from top-left corner in horizontal direction + * @param iY taget position, number of pixels from top-left corner in vertical direction + * @param iVideoWidth video image width, e.g: 1920, 1280 + * @param iVideoHeight video image height, e.g: 1080, 720 + * @details we usually use VLK_TRACK_TEMPLATE_SIZE_AUTO for pParam->emTrackTempSize and VLK_SENSOR_VISIBLE1 for pParam->emTrackSensor + */ +VLK_API void VLK_CALL VLK_TrackTargetPositionEx(const VLK_TRACK_MODE_PARAM* pParam, int iX, int iY, int iVideoWidth, int iVideoHeight); + +/** @brief Disable track mode + */ +VLK_API void VLK_CALL VLK_DisableTrackMode(); + +/** @brief Focus in + * @param sSpeed the speed of focus in + * @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster + * this function is available only when the Gimbal is in Manual focusing mode, call VLK_GetFocusMode to + * get current focus mode, call VLK_SetFocusMode to set focus mode + */ +VLK_API void VLK_CALL VLK_FocusIn(short sSpeed); + +/** @brief Focus out + * @param sSpeed the speed of focus out + * @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster + * this function is available only when the Gimbal is in Manual focusing mode, call VLK_GetFocusMode to + * get current focus mode, call VLK_SetFocusMode to set focus mode + */ +VLK_API void VLK_CALL VLK_FocusOut(short sSpeed); + +/** @brief Stop focus + * @details once you call VLK_FocusIn or VLK_FocusOut, Camera will keep focusing until it reach the limitation or VLK_StopFocus is called + */ +VLK_API void VLK_CALL VLK_StopFocus(); + +/** @brief Move to home position + */ +VLK_API void VLK_CALL VLK_Home(); + +/** @brief Switch motor + * @param iOn turn on/turn off \n + * 1 turn on \n + * 0 turn off + */ +VLK_API void VLK_CALL VLK_SwitchMotor(int iOn); + +/** @brief Check motor status + * @return current motor status + * @retval 1 on + * @retval 0 off + */ +VLK_API int VLK_CALL VLK_IsMotorOn(); + +/** @brief Enable Gimbal follow drone + * @param iEnable enable or disable \n + * 1 enable \n + * 0 disable + */ +VLK_API void VLK_CALL VLK_EnableFollowMode(int iEnable); + +/** @brief Check follow mode + * @return current follow status + * @retval 1 enabled + * @retval 0 disabled + */ +VLK_API int VLK_CALL VLK_IsFollowMode(); + +/** @brief Turn to specific yaw and pitch + * @param dYaw specific yaw, must be a double value from VLK_YAW_MIN to VLK_YAW_MAX + * @param dPitch specific pitch, must be a double value from VLK_PITCH_MIN to VLK_PITCH_MAX + */ +VLK_API void VLK_CALL VLK_TurnTo(double dYaw, double dPitch); + +/** @brief Enable track + */ +VLK_API void VLK_CALL VLK_StartTrack(); + + +/** @brief Disable track + */ +VLK_API void VLK_CALL VLK_StopTrack(); + +/** @brief Check if track is enabled + * @return current track status + * @retval 1 enabled + * @retval 0 disabled + */ +VLK_API int VLK_CALL VLK_IsTracking(); + +/** @brief Set track template size + * @param emSize track template size enumeration + * @see VLK_TRACK_TEMPLATE_SIZE + */ +VLK_API void VLK_CALL VLK_SetTrackTemplateSize(VLK_TRACK_TEMPLATE_SIZE emSize); + +/** @brief Set image color + * @param emImgType image type enumeration + * @param iEnablePIP enable picture in picture \n + * 1 enable \n + * 0 disable + * @param emIRColor IR color enumeration + * @see VLK_IMAGE_TYPE + */ +VLK_API void VLK_CALL VLK_SetImageColor(VLK_IMAGE_TYPE emImgType, int iEnablePIP, VLK_IR_COLOR emIRColor); + +/** @brief Ask Gimbal take a photograph + * @details make sure there is SD card in the Gimbal + */ +VLK_API void VLK_CALL VLK_Photograph(); + +/** @brief Gimbal start or stop recording + * @param iOn \n + * 1 start recording \n + * 0 stop recording + * @details make sure there is SD card in the Gimbal + */ +VLK_API void VLK_CALL VLK_SwitchRecord(int iOn); + +/** @brief Check if Gimbal is recording + * @return flag of recording status + * @retval 1 recording + * @retval 0 not recording + * @details make sure there is SD card in the Gimbal + */ +VLK_API int VLK_CALL VLK_IsRecording(); + +/** @brief Enable defog + * @param iOn \n + * 1 defog is enabled \n + * 0 defog is disabled + */ +VLK_API void VLK_CALL VLK_SwitchDefog(int iOn); + +/** @brief Check if defog is enabled + * @return flag of defog status + * @retval 1 defog is enabled + * @retval 0 defog is disabled + */ +VLK_API int VLK_CALL VLK_IsDefogOn(); + +/** @brief Set record mode + * @param emMode record mode enumeration + * @details some models could not take photo while it is recording, + * they must be switched to a certain + * @see VLK_RECORD_MODE + */ +VLK_API void VLK_CALL VLK_SetRecordMode(VLK_RECORD_MODE emMode); + +/** @brief Get current record mode + * @return record mode enumeration + * @see VLK_RECORD_MODE + */ +VLK_API int VLK_CALL VLK_GetRecordMode(); + +/** @brief Set focus mode ( manual focus or automatic focus) + * @param focus mode enumeration + * @see VLK_FOCUS_MODE + */ +VLK_API void VLK_CALL VLK_SetFocusMode(VLK_FOCUS_MODE emMode); + +/** @brief Get current focus mode ( manual focus or automatic focus) + * @return focus mode enumeration + * @see VLK_FOCUS_MODE + */ +VLK_API int VLK_CALL VLK_GetFocusMode(); + +/** @brief Zoom to a specific magnification + * @param fMag specific magnification + * @details the camera will reach the limitation if the specific magnification over it's capability + */ +VLK_API void VLK_CALL VLK_ZoomTo(float fMag); + +/** @brief IR digital zoom in + * @param sSpeed deprecated, pass 0 + * @details zoom in x1 then stop automatically + */ +VLK_API void VLK_CALL VLK_IRDigitalZoomIn(short sSpeed); + +/** @brief IR digital zoom out + * @param sSpeed deprecated, pass 0 + * @details zoom out x1 then stop automatically + */ +VLK_API void VLK_CALL VLK_IRDigitalZoomOut(short sSpeed); + +/** @brief Switch EO digital zoom status + * @param iOn \n + * 1 enable \n + * 0 disable + */ +VLK_API void VLK_CALL VLK_SwitchEODigitalZoom(int iOn); + +/** @brief Enter S-BUS mode + * @deprecated + */ +VLK_API void VLK_CALL VLK_EnterSBUSMode(); + +/** @brief Exit S-BUS mode + * @deprecated + */ +VLK_API void VLK_CALL VLK_ExitSBUSMode(); + +/** @brief Query device configuration + * @details this function is asynchronous, configuration data will be passed back throw VLK_DevStatus_CB + * @see VLK_DevStatus_CB + */ +VLK_API void VLK_CALL VLK_QueryDevConfiguration(); + +/** @brief Set OSD + * @param pParam OSG configuration + * @details we don't provide VLK_GetOSD because OSD is included in device configuration + * @see VLK_OSD_PARAM + */ +VLK_API void VLK_CALL VLK_SetOSD(const VLK_OSD_PARAM* pParam); + +/** @brief Set wireless control channels map + * @param pChnlsMap wireless control channels map configuration + * @details we don't provide VLK_GetWirelessCtrlChnlMap because channels map is included in device configuration + * @see VLK_CHANNELS_MAP + */ +VLK_API void VLK_CALL VLK_SetWirelessCtrlChnlMap(const VLK_CHANNELS_MAP* pChnlsMap); + +/** @brief Send extent command + * @param szCmd command data pointer + * @param iLen command data length + * @details for some seldom-used Gimbal control command which we didn't provide specific interface, + * you should call this function to send command data directly + */ +VLK_API void VLK_CALL VLK_SendExtentCmd(const char* szCmd, int iLen); + + +/** @name Laser Control functions + * @{ + */ + +/** @brief Switch laser status + * @param iOn \n + * 1 turn on \n + * 0 turn off + * @details make sure your device has laser capability + */ +VLK_API void VLK_CALL VLK_SwitchLaser(int iOn); + +/** @brief Laser single ranging + */ +VLK_API void VLK_CALL VLK_LaserSingle(); + + +/** @brief Laser zoom in + * @param sSpeed deprecated, pass 0 + */ +VLK_API void VLK_CALL VLK_LaserZoomIn(short sSpeed); + +/** @brief Laser zoom out + * @param sSpeed deprecated, pass 0 + */ +VLK_API void VLK_CALL VLK_LaserZoomOut(short sSpeed); + +/** @brief Laser stop zoom + */ +VLK_API void VLK_CALL VLK_LaserStopZoom(); + +/** @brief Set laser zoom mode + * @param emMode laser zoom mode enumeration + */ +VLK_API void VLK_CALL VLK_SetLaserZoomMode(VLK_LASER_ZOOM_MODE emMode); +/** @} Laser Control functions */ + +#ifdef __cplusplus +} +#endif + +#endif //__VIEW_LINK_H__ diff --git a/src/gimbal_base.hpp b/src/gimbal_base.hpp new file mode 100644 index 0000000..33fe7a2 --- /dev/null +++ b/src/gimbal_base.hpp @@ -0,0 +1,454 @@ +#ifndef gimbal_base +#define gimbal_base +#include +#include +#include +#include +// #include +#include + +class GimbalBase +{ + friend std::ostream &operator<<(std::ostream &output, GimbalBase *gimbal); + +public: + static std::string time_string_now(); + + + // vel rpy + GimbalBase() = delete; + GimbalBase(std::string save_path = "gimbal_save_data", std::string device_name = "", int max_zoom = 0x4000, int min_zoom = 0x0000) + : past_clock(std::chrono::system_clock::now()), past_clock_zoom(std::chrono::system_clock::now()), max_zoom(max_zoom), min_zoom(min_zoom), save_path(save_path), device_name(device_name) + { + + this->save_path = save_path[save_path.size() - 1] == '/' ? save_path : save_path + '/'; + this->save_path = this->save_path[0] == '/' ? this->save_path : std::string("/") + this->save_path; + this->save_path = std::string(getenv("HOME")) + this->save_path; + std::cout << "Data Save Path: " << this->save_path << std::endl; + if (access(this->save_path.c_str(), 0x00) != 0) + { + mkdir(this->save_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR | S_IRWXG | S_IRWXO); + if (access(this->save_path.c_str(), 0x00) != 0) + { + throw std::runtime_error("Create dir for save DIR failed!"); + } + } + }; + GimbalBase(const GimbalBase &) = delete; + virtual ~GimbalBase() = default; + // 派生类必须要实现到方法 + virtual void open_tty(std::string tty_port = "/dev/ttyUSB0") = 0; + virtual void open_camera(std::string camera_id = "0", int width = 640, int height = 480, int fps=30) = 0; + virtual bool get_info(float ret[12]) = 0; // 入口 + virtual void ctl_gimbal(uint8_t rpy_ctl[3], double rpy_vel[3], bool stop_search = true) = 0; + virtual void home() = 0; + virtual void hold() = 0; + virtual void fellow() = 0; + + // 基类已有基础功能 + virtual void tracking(float xy_info[6], int ctl_roll = 0, float kp = 0.1, float ki = 0.0, float kd = 0.0, float ignore_error = 35); + virtual bool search(bool is_roll, float search_vision_h = 50, float search_vision_v = 30, float search_center_h = 0, float search_center_v = 45); + virtual bool start_record(); + virtual void finish_record(); + virtual bool get_picture(std::string name = ""); + virtual float get_dt(); + + // 派生类可选 + virtual void focus_out(){}; + virtual void focus_in(){}; + virtual void focus_stop(){}; + virtual void zoom_in(){}; + virtual void zoom_out(){}; + virtual void zoom_stop(){}; + virtual uint16_t get_zoom_val(){}; + virtual bool auto_zoom(float curr_area, uint16_t now_zoom_val, float thresh_area_up = 0.1, float thresh_area_down = 0.5){}; + + // 读取视频流 + cv::VideoCapture &operator>>(cv::Mat &mat); + cv::VideoCapture cap; + // 保存视频 + cv::VideoWriter outvideo; + // 当前正在处理的帧 + cv::Mat frame; + + // 当期状态 + enum + { + MANUAL_CONTROL = 0, + HOME_BACK, + TRACKING, + YAW_FOLLOW, + HOLDING, + SEARCHING + } move_state; + + enum + { + ZOOM_KEEP = 0, + ZOOM_OUT, + ZOOM_IN, + ZOOM_AUTO + } zoom_state; + enum + { + INTERACTIVE_FEEDBACK = 0, + AUTO_FEEDBACK + } feedback_state; + + uint16_t zoom_val = 0; + float fzoom_val = 0.; + bool is_recording = false; + + std::string save_path; + const uint16_t max_zoom; + const uint16_t min_zoom; + + // 停止 + bool is_stop_auto = false; + + // 吊舱角度信息 + float past_stator_rel_camera[3]; + float gimbal_state[12]; + float gimbal_state_paste[12]; + /* + std::cout << "----------------------gimbal-now-status-------------------------" << std::endl; + std::cout << "imu_roll: " << std::setprecision(6) << ret[0] << std::endl; + std::cout << "imu_pitch: " << std::setprecision(6) << ret[1] << std::endl; + std::cout << "imu_yaw: " << std::setprecision(6) << ret[2] << std::endl; + std::cout << "target_roll: " << std::setprecision(6) << ret[3] << std::endl; + std::cout << "target_pitch: " << std::setprecision(6) << ret[4] << std::endl; + std::cout << "target_yaw: " << std::setprecision(6) << ret[5] << std::endl; + std::cout << "rel_roll: " << std::setprecision(6) << ret[6] << std::endl; + std::cout << "rel_pitch: " << std::setprecision(6) << ret[7] << std::endl; + std::cout << "rel_yaw: " << std::setprecision(6) << ret[8] << std::endl; + std::cout << "imu_roll_vel: " << std::setprecision(6) << ret[9] << std::endl; + std::cout << "imu_pitch_vel: " << std::setprecision(6) << ret[10] << std::endl; + std::cout << "imu_yaw_vel: " << std::setprecision(6) << ret[11] << std::endl; + */ + + const std::string device_name; + +protected: + std::chrono::system_clock::time_point past_clock; + // 保存上次查询放大倍数时间 + std::chrono::system_clock::time_point past_clock_zoom; + + int camera_width; + int camera_hight; + + // 用于中断吊舱搜索到线程 + bool stop_search_; + // 吊仓搜索路线 + float search_line[4][2]; + std::thread *t_search = nullptr; + + void _search(bool is_roll = false); + // 用于再吊舱进行转动搜索时判定，是否到达期望值。 + // q10f控制的是电机角度， g1控制的是yaw电机角度，roll,pitch到imu角度 + double control_feedback_truth[3]; +}; + +bool GimbalBase::search(bool is_roll, float search_vision_h, float search_vision_v, float search_center_h, float search_center_v) +{ + float ignore_error = 5; + this->search_line[0][0] = search_center_v - search_vision_v / 2 - ignore_error; + this->search_line[0][1] = search_center_h - search_vision_h / 2 - ignore_error; + + this->search_line[1][0] = search_center_v - search_vision_v / 2 - ignore_error; + this->search_line[1][1] = search_center_h + search_vision_h / 2 + ignore_error; + + this->search_line[2][0] = search_center_v + search_vision_v / 2 + ignore_error; + this->search_line[2][1] = search_center_h + search_vision_h / 2 + ignore_error; + + this->search_line[3][0] = search_center_v + search_vision_v / 2 + ignore_error; + this->search_line[3][1] = search_center_h - search_vision_h / 2 - ignore_error; + // 判定是否存在 + if (t_search && t_search->joinable()) + { + this->stop_search_ = true; + t_search->join(); + } + this->stop_search_ = false; + t_search = new std::thread(&GimbalBase::_search, this, is_roll); + this->move_state = SEARCHING; + return t_search->joinable(); +} + +void GimbalBase::_search(bool is_roll) +{ + float t = 0; + uint8_t rpy_ctl[3]{2, 2, 2}; + double rpy_val[3]{0, 0, 0}; + while (true) + { + if (this->stop_search_) + { + return; + } + for (int i = 0; i < 4 && !this->stop_search_; ++i) + { + if (is_roll) + { + rpy_val[0] = this->search_line[i][1]; + rpy_val[1] = this->search_line[i][0]; + rpy_val[2] = 0; + } + else + { + rpy_val[0] = 0; + rpy_val[1] = this->search_line[i][0]; + rpy_val[2] = this->search_line[i][1]; + } + + // 等待到达指定角度 + while (true) + { + if (this->stop_search_) + { + return; + } + this->ctl_gimbal(rpy_ctl, rpy_val, false); + std::cout << "\033[33m" + << "Wait for go to the point" + << "\033[30m" + << "\n" + << "target_roll: " << rpy_val[0] << " " + << "current_roll: " << this->control_feedback_truth[0] << "\n" + << "target_pitch: " << rpy_val[1] << " " + << "current_pitch: " << this->control_feedback_truth[1] << "\n" + << "target_yaw: " << rpy_val[2] << " " + << ":current_yaw: " << this->control_feedback_truth[2] << "\n" + << std::endl; + using namespace std::chrono_literals; + std::this_thread::sleep_for(10ms); + if (is_roll) + { + if (std::fabs(rpy_val[0] - this->control_feedback_truth[0]) < 3 && std::fabs(rpy_val[1] - this->control_feedback_truth[1]) < 3) + break; + } + else + { + if (std::fabs(rpy_val[2] - this->control_feedback_truth[2]) < 3 && std::fabs(rpy_val[1] - this->control_feedback_truth[1]) < 3) + break; + } + } + } + } +} + +void GimbalBase::tracking(float xy_info[2], int ctl_roll, float kp, float ki, float kd, float ignore_error) +{ + // std::cout << "PID: " << kp << " " << ki << " " << kd << std::endl; + static std::chrono::system_clock::time_point paste_track_ts = std::chrono::system_clock::now(); + static float i_x = 0, i_y = 0; + + std::chrono::system_clock::time_point now_ts = std::chrono::system_clock::now(); + // TODO: 就很离谱，有时候会dt值会变成0.0001几，先使用指数平均顶着 + std::chrono::duration dt = now_ts - paste_track_ts; + static std::chrono::duration filter_dt = dt; + if (dt.count() > 0.01) + filter_dt = 0.5 * filter_dt + 0.5 * dt; + + float x = xy_info[0]; + float y = xy_info[1]; + static float paste_xy[2]{x, y}; + + float dt_error_x = x - paste_xy[0]; + float dt_error_y = y - paste_xy[1]; + static float dt_error[2]{dt_error_x, dt_error_y}; + + // 重置 + if (std::abs(std::sqrt(x * x + y * y) - std::sqrt(paste_xy[0] * paste_xy[0] + paste_xy[1] * paste_xy[1])) > 50) + { + paste_xy[0] = x; + paste_xy[1] = y; + dt_error_x = 0; + dt_error_y = 0; + dt_error[0] = 0; + dt_error[1] = 0; + i_x = 0; + i_y = 0; + // std::cout << ignore_error <<" :重置历史数据: " << std::abs(std::sqrt(x * x + y * y) - std::sqrt(paste_xy[0] * paste_xy[0] + paste_xy[1] * paste_xy[1])) << std::endl; + } + + float vel_x = (dt_error_x - dt_error[0]) / filter_dt.count(); + float vel_y = (dt_error_y - dt_error[1]) / filter_dt.count(); + i_x += dt_error_x * filter_dt.count(); + i_y += dt_error_y * filter_dt.count(); + + paste_track_ts = now_ts; + paste_xy[0] = x; + paste_xy[1] = y; + dt_error[0] = dt_error_x; + dt_error[1] = dt_error_y; + // std::cout << "vel_x: " << vel_x << " vel_y: " << vel_y << " dt: " << dt.count() << std::endl; + // kd = 0; + + // TODO 没有提供vel_x, i_x时自己计算 + // std::cout << "像素误差: " << x << " " << y << std::endl; + // ignore_error = ignore_error * (get_zoom_val() / max_zoom); + x = std::abs(x) > std::fmax(ignore_error, 10) ? (x > 0 ? 1 : -1) * fmax(std::abs(x) * kp, 1) : 0; + y = std::abs(y) > std::fmax(ignore_error, 10) ? (y > 0 ? 1 : -1) * fmax(std::abs(y) * kp, 1) : 0; + // std::cout << "像素误差: " << x << " " << y << std::endl; + + double mix_control = (x == 0 || std::abs(x) == 1) ? x : x + ki * i_x + kd * vel_x; + double pitch_control = (y == 0 || std::abs(y) == 1) ? y : y + ki * i_y + kd * vel_y; + + // std::cout << "最终速度: " << pitch_control << " " << mix_control << std::endl; + uint8_t ctl[3]; + double vel[3]; + switch (ctl_roll) + { + case 0: // yaw + pitch + ctl[0] = 2; + ctl[1] = 1; + ctl[2] = 1; + vel[0] = 0; + vel[1] = pitch_control; + vel[2] = mix_control; + break; + case 1: // roll + pitch + ctl[0] = 1; + ctl[1] = 1; + ctl[2] = 2; + vel[0] = -mix_control; + vel[1] = pitch_control; + vel[2] = 0; + break; + case 2: // 混合控制 + /* + "imu_roll: ret[0] + "imu_pitch: ret[1] + "imu_yaw: ret[2] + "target_roll: ret[3] + "target_pitch: ret[4] + "target_yaw: ret[5] + "rel_roll: ret[6] + "rel_pitch: ret[7] + "rel_yaw: ret[8] + "imu_roll_vel: ret[9] + "imu_pitch_vel: ret[10] + "imu_yaw_vel: ret[11] + "rel_roll_vel: ret[12] + "rel_pitch_vel: ret[13] + "rel_yaw_vel: ret[14] + */ + // static bool is_switch = true; + // if (this->gimbal_state[7] < 70) // pitch + roll + // { + // if (is_switch == true) + // { + // is_switch = false; + // } + // else + // { + // } + // /* + // 1. roll = (if yaw > 0 ? -1 : 1 ) * (90 - abs(yaw)) + // 2. yaw = 0 + // */ + // } + // else // pitch + yaw + // { + // if (is_switch == false) + // { + // is_switch = true; + // } + // else + // { + // } + // /* + // 1. yaw = (if roll > 0 ? -1 : 1 ) * (90 - abs(roll)) + // 2. roll = 0 + // */ + // } + break; + } + this->ctl_gimbal(ctl, vel); + this->move_state = TRACKING; +} + +float GimbalBase::get_dt() +{ + std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = past_clock - now; + past_clock = now; + return elapsed_seconds.count(); +}; + +std::ostream &operator<<(std::ostream &output, GimbalBase *gimbal) +{ + while (true) + { + float ret[12]; + // std::memset(ret, 0, 15); + if (!gimbal->get_info(ret)) + continue; + std::cout << "----------------------gimbal-now-status-------------------------" << std::endl; + std::cout << "imu_roll: " << std::setprecision(6) << ret[0] << std::endl; + std::cout << "imu_pitch: " << std::setprecision(6) << ret[1] << std::endl; + std::cout << "imu_yaw: " << std::setprecision(6) << ret[2] << std::endl; + std::cout << "target_roll: " << std::setprecision(6) << ret[3] << std::endl; + std::cout << "target_pitch: " << std::setprecision(6) << ret[4] << std::endl; + std::cout << "target_yaw: " << std::setprecision(6) << ret[5] << std::endl; + std::cout << "rel_roll: " << std::setprecision(6) << ret[6] << std::endl; + std::cout << "rel_pitch: " << std::setprecision(6) << ret[7] << std::endl; + std::cout << "rel_yaw: " << std::setprecision(6) << ret[8] << std::endl; + std::cout << "imu_roll_vel: " << std::setprecision(6) << ret[9] << std::endl; + std::cout << "imu_pitch_vel: " << std::setprecision(6) << ret[10] << std::endl; + std::cout << "imu_yaw_vel: " << std::setprecision(6) << ret[11] << std::endl; + break; + } + return output; +}; + +cv::VideoCapture &GimbalBase::operator>>(cv::Mat &mat) +{ + cap >> frame; + mat = frame; + if (outvideo.isOpened()) + { + outvideo << frame.clone(); + } + return cap; +}; + +bool GimbalBase::start_record() +{ + if (!outvideo.isOpened()) + { + std::string video_path = this->save_path + time_string_now() + ".mp4"; + std::string pipeline = "appsrc ! autovideoconvert ! omxh265enc ! matroskamux ! filesink location=" + video_path; + outvideo.open(pipeline, CV_FOURCC('M', 'P', '4', 'V'), 25.0, cv::Size(this->camera_width, this->camera_hight)); + this->is_recording = true; + } + return outvideo.isOpened(); +} +void GimbalBase::finish_record() +{ + this->is_recording = false; + outvideo.release(); +} + +bool GimbalBase::get_picture(std::string name) +{ + std::string img_path; + if (name.empty()) + { + img_path = this->save_path + time_string_now() + ".png"; + } + else + { + img_path = this->save_path + name + ".png"; + } + return cv::imwrite(img_path.c_str(), frame); +} +inline std::string GimbalBase::time_string_now() +{ + time_t t = std::time(NULL); + tm *local = localtime(&t); + char buf[64]; + std::strftime(buf, 64, "%Y-%m-%d_%H:%M:%S", local); + return buf; +} +#endif diff --git a/src/gimbal_server.cpp b/src/gimbal_server.cpp new file mode 100644 index 0000000..39c1fde --- /dev/null +++ b/src/gimbal_server.cpp @@ -0,0 +1,407 @@ +#include +#include +#include +#include +#include +#include "gimbal.h" +#include "ViewLink.h" +#include "amov_gimbal.h" +#include "prometheus_gimbal_control/GimbalControl.h" +#include "prometheus_gimbal_control/GimbalState.h" +#include "prometheus_gimbal_control/VisionDiff.h" +#include +#include +#include +#include +#include + +// amovGimbal::gimbal *gimbal = nullptr; +GimbalBase *g_gimbal = nullptr; + +prometheus_gimbal_control::GimbalState g_pub_info; +bool g_stop_track = false; + +void ctlCallback(const prometheus_gimbal_control::GimbalControl::ConstPtr &ctl) +{ + switch (ctl->rpyMode) + { + case prometheus_gimbal_control::GimbalControl::manual: + uint8_t rpy_ctl[3]; + double rpy_vel[3]; + rpy_ctl[0] = ctl->roll; + rpy_ctl[1] = ctl->yaw; + rpy_ctl[2] = ctl->pitch; + rpy_vel[0] = ctl->rValue; + rpy_vel[1] = ctl->pValue; + rpy_vel[2] = ctl->yValue; + + + + g_gimbal->ctl_gimbal(rpy_ctl, rpy_vel); + + + ROS_INFO_STREAM("Manual " + << "rpy control mode: " << (int)rpy_ctl[0] << " " << (int)rpy_ctl[1] << " " << (int)rpy_ctl[2] + << " value: " << rpy_vel[0] << " " << rpy_vel[1] << " " << rpy_vel[2]); + + break; + case prometheus_gimbal_control::GimbalControl::home: + g_gimbal->home(); + ROS_INFO_STREAM("Back Home"); + break; + case prometheus_gimbal_control::GimbalControl::hold: + g_gimbal->hold(); + ROS_INFO_STREAM("Hold"); + break; + case prometheus_gimbal_control::GimbalControl::fellow: + g_gimbal->fellow(); + ROS_INFO_STREAM("Fellow"); + break; + } + switch (ctl->focusMode) + { + case prometheus_gimbal_control::GimbalControl::focusIn: + g_gimbal->focus_in(); + ROS_INFO_STREAM("Focus In"); + break; + case prometheus_gimbal_control::GimbalControl::focusOut: + g_gimbal->focus_out(); + ROS_INFO_STREAM("Focus Out"); + break; + case prometheus_gimbal_control::GimbalControl::focusStop: + g_gimbal->focus_stop(); + ROS_INFO_STREAM("Focus Stop"); + break; + } + switch (ctl->zoomMode) + { + case prometheus_gimbal_control::GimbalControl::zoomIn: + g_gimbal->zoom_in(); + ROS_INFO_STREAM("Zoom In"); + break; + + case prometheus_gimbal_control::GimbalControl::zoomOut: + g_gimbal->zoom_out(); + ROS_INFO_STREAM("Zoom Out"); + break; + case prometheus_gimbal_control::GimbalControl::zoomStop: + g_gimbal->zoom_stop(); + ROS_INFO_STREAM("Zoom Stop"); + break; + } +}; + +void trackCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr ¢) +{ + if (g_stop_track) + return; + float tmp_cent[2]{0, 0}; + tmp_cent[0] = cent->objectX + cent->objectWidth / 2 - cent->frameWidth / 2; + tmp_cent[1] = cent->objectY + cent->objectHeight / 2 - cent->frameHeight / 2; + int roll_mode = cent->ctlMode; + float kp = cent->kp; + float ki = cent->ki; + float kd = cent->kd; + + static bool flag_zoom = false; + static bool flag_hold = false; + + if (cent->detect != 0) + { + int zoom_val = cent->currSize ? std::fmax(g_gimbal->zoom_val, 1) : 1; + float scale = std::fmax(g_gimbal->max_zoom - zoom_val, 1.) / g_gimbal->max_zoom; + g_gimbal->tracking(tmp_cent, roll_mode, kp * scale, ki, kd, cent->trackIgnoreError * zoom_val / g_gimbal->max_zoom); + // std::cout << "step 3 : " << std::endl; + if (cent->autoZoom && cent->currSize && tmp_cent[0] < cent->trackIgnoreError && tmp_cent[1] < cent->trackIgnoreError) + { + g_gimbal->auto_zoom(cent->currSize, zoom_val, cent->maxSize, cent->minSize); + flag_zoom = true; + } + // std::cout << "step 4 : " << std::endl; + flag_hold = true; + } + else + { + if (cent->currSize && flag_zoom) + { + g_gimbal->zoom_stop(); + flag_zoom = false; + } + if (flag_hold) + { + g_gimbal->hold(); + flag_hold = false; + } + } +}; + +void currSizeCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr &info) +{ + static bool flag = false; + int now_flag = 0; + int zoom_val = std::fmax(g_gimbal->zoom_val, 1); + if (info->detect != 0 && (std::abs(g_pub_info.imuAngleVel[0]) + std::abs(g_pub_info.imuAngleVel[1]) + std::abs(g_pub_info.imuAngleVel[2])) < 3) + { + g_gimbal->auto_zoom(info->currSize, zoom_val, info->maxSize, info->minSize); + flag = true; + } + else + { + if (flag) + { + g_gimbal->zoom_stop(); + flag = false; + } + } +} + +bool videoRecord(std_srvs::SetBool::Request &ref, std_srvs::SetBool::Response &req) +{ + if (ref.data) + { + g_gimbal->start_record(); + req.success = g_gimbal->outvideo.isOpened(); + ROS_INFO_STREAM("Start Video Record ! "); + } + else + { + if (!g_gimbal->outvideo.isOpened()) + { + req.success = false; + req.message = "video record not is started ! "; + ROS_WARN_STREAM("video record not is started ! "); + } + g_gimbal->finish_record(); + if (g_gimbal->outvideo.isOpened()) + { + req.success = false; + req.message = "finish video fail ! "; + ROS_WARN_STREAM("finish video fail ! "); + } + else + { + req.success = true; + ROS_INFO_STREAM("Finish Video Record ! "); + } + } + return true; +} + +void picCallback(const std_msgs::String::ConstPtr &info) +{ + ROS_INFO("Get Picture "); + g_gimbal->get_picture(); +} + +void callAll(image_transport::Publisher *image_pub) +{ + cv::Mat frame; + while (true) + { + (*g_gimbal) >> frame; + if (!frame.empty()) + { + image_pub->publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg()); + } + std::this_thread::sleep_for(10ms); + } +} + +bool search(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp) +{ + resp.success = g_gimbal->search(req.data); + return true; +} + +// 切换反馈模式，自动？交换 +bool switchTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp) +{ + static_cast(g_gimbal)->isGetRealVelocity = req.data; + resp.success = true; + resp.message = static_cast(g_gimbal)->isGetRealVelocity ? "return real angle speed" : "return compute angle speed"; + return true; +} + +// 屏蔽tracking控制 +bool stopTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp) +{ + g_stop_track = req.data; + resp.success = true; + return true; +} + +int VLK_DevStatusCallback(int iType, const char* szBuffer, int iBufLen, void* pUserParam) +{ + if (VLK_DEV_STATUS_TYPE_MODEL == iType) + { + VLK_DEV_MODEL* pModel = (VLK_DEV_MODEL*)szBuffer; + std::cout << "model code: " << pModel->cModelCode << ", model name: " << pModel->szModelName << std::endl; + } + else if (VLK_DEV_STATUS_TYPE_CONFIG == iType) + { + VLK_DEV_CONFIG* pDevConfig = (VLK_DEV_CONFIG*)szBuffer; + std::cout << "VersionNO: " << pDevConfig->cVersionNO << ", DeviceID: " << pDevConfig->cDeviceID << ", SerialNO: " << pDevConfig->cSerialNO << std::endl; + } + else if (VLK_DEV_STATUS_TYPE_TELEMETRY == iType) + { + /* + * once device is connected, telemetry information will keep updating, + * in order to avoid disturbing user input, comment out printing telemetry information + */ + // VLK_DEV_TELEMETRY* pTelemetry = (VLK_DEV_TELEMETRY*)szBuffer; + // cout << "Yaw: " << pTelemetry->dYaw << ", Pitch: " << pTelemetry->dPitch << ", sensor type: " << pTelemetry->emSensorType << ", Zoom mag times: " << pTelemetry->sZoomMagTimes << endl; + } + else + { + std::cout << "error: unknown status type: " << iType << std::endl; + } + + + return 0; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "gimbal"); + ros::NodeHandle n("~"); + image_transport::ImageTransport it(n); + image_transport::Publisher image_pub; + cv::Mat frame; + sensor_msgs::ImagePtr img; + // rpy控制, 变焦, 放大 + image_pub = it.advertise("image_raw", 1); + ros::Subscriber sub_ctl = n.subscribe("control", 10, ctlCallback); + ros::Subscriber sub_track = n.subscribe("track", 10, trackCallback); + ros::Subscriber save_pic = n.subscribe("get_pic", 100, picCallback); + ros::Publisher pub_rpy = n.advertise("state", 10); + ros::ServiceServer srv_search = n.advertiseService("search", search); + ros::ServiceServer srv_video = n.advertiseService("record_video", videoRecord); + ros::ServiceServer srv_track = n.advertiseService("stop_track", stopTrack); + + ros::Rate rate(20); + + int camera_width, camera_height, uav_id; + std::string camera_id; + std::string tty_url, save_path, gimbal_type; + n.param("camera_id", camera_id, "0"); + n.param("uav_id", uav_id, 0); // 无人机的ID + n.param("tty_url", tty_url, "/dev/ttyUSB0"); + n.param("camera_width", camera_width, 1920); + n.param("camera_height", camera_height, 1080); + n.param("save_path", save_path, "gimbal_video_data"); + n.param("gimbal_type", gimbal_type, "at10"); + + if (gimbal_type == "q10f") + { + g_gimbal = new GimbalQ10f(save_path); + } + else if (gimbal_type == "g1") + { + g_gimbal = new AmovG1(save_path); + } + else if (gimbal_type == "at10") + { + g_gimbal = new GimbalAT10(save_path); + } + else + { + ROS_ERROR_STREAM("NOT SUPPORT " << gimbal_type); + return -1; + } + + try + { + g_gimbal->open_tty(tty_url); + } + catch (...) + { + ROS_ERROR_STREAM("Unable to open Serial Port ! "); + getchar(); + return -1; + } + + try + { + // 以60帧率打开，仅对AMOV G1有效 + g_gimbal->open_camera(camera_id, camera_width, camera_height, 60); + } + catch (...) + { + ROS_ERROR_STREAM("Unable to open Camera ! "); + getchar(); + return -1; + } + + if (g_gimbal->device_name == "q10f" || g_gimbal->device_name == "at10") + { + static ros::ServiceServer srv_track_mode = n.advertiseService("track_mode", switchTrack); + static ros::Subscriber sub_currSize = n.subscribe("currSize", 10, currSizeCallback); + static_cast(g_gimbal)->stopAuto(); + static_cast(g_gimbal)->stopAuto(); + // initialize sdk + int iRet = VLK_Init(); + if (VLK_ERROR_NO_ERROR != iRet) + { + std::cout << "VLK_Init failed, error: " << iRet << std::endl; + return -1; + } + // register device status callback + VLK_RegisterDevStatusCB(VLK_DevStatusCallback, NULL); + // ROS_INFO_STREAM("AT10 !"); + // 5秒后停止 zoom_out() + static ros::Timer stop_zoom = n.createTimer( + ros::Duration(5.0), [&](const ros::TimerEvent &) + { g_gimbal->zoom_out(); }, + true); + } + + g_gimbal->home(); + g_gimbal->zoom_out(); + ROS_INFO_STREAM("Program Started ! "); + std::thread spin(callAll, &image_pub); + if(g_gimbal->device_name == "AT10") + { + ROS_INFO_STREAM("at10 !"); + }else if(g_gimbal->device_name == "q10f") + { + ROS_INFO_STREAM("q10f !"); + }else if(g_gimbal->device_name == "g1"){ + ROS_INFO_STREAM("g1 !"); + // print sdk version + std::cout << "ViewLink SDK version: " << GetSDKVersion() << std::endl; + }else { + ROS_INFO_STREAM("unknow device!!!"); + std::cout << "device name: " << g_gimbal->device_name << std::endl; + std::cout << "device type: " << gimbal_type << std::endl; + } + + + g_pub_info.Id = uav_id; + while (ros::ok()) + { + ros::spinOnce(); + float info[12]; + g_gimbal->get_info(info); + g_pub_info.mode = g_gimbal->move_state; + g_pub_info.feedbackMode = g_gimbal->feedback_state; + g_pub_info.isRecording = g_gimbal->is_recording; + g_pub_info.zoomState = g_gimbal->zoom_state; + g_pub_info.zoomVal = g_gimbal->fzoom_val; + g_pub_info.imuAngle[0] = info[0]; + g_pub_info.imuAngle[1] = info[1]; + g_pub_info.imuAngle[2] = info[2]; + g_pub_info.rotorAngleTarget[0] = info[3]; + g_pub_info.rotorAngleTarget[1] = info[4]; + g_pub_info.rotorAngleTarget[2] = info[5]; + g_pub_info.rotorAngle[0] = info[6]; + g_pub_info.rotorAngle[1] = info[7]; + g_pub_info.rotorAngle[2] = info[8]; + g_pub_info.imuAngleVel[0] = info[9]; + g_pub_info.imuAngleVel[1] = info[10]; + g_pub_info.imuAngleVel[2] = info[11]; + pub_rpy.publish(g_pub_info); + rate.sleep(); + // std::cout << "run ........" << std::endl; + } + delete g_gimbal; +} diff --git a/src/serial.cpp b/src/serial.cpp new file mode 100644 index 0000000..a1dae49 --- /dev/null +++ b/src/serial.cpp @@ -0,0 +1,293 @@ +#include + +#if !defined(_WIN32) && !defined(__OpenBSD__) && !defined(__FreeBSD__) +#include +#endif + +#include "serial/serial.h" + +#include "serial/impl/unix.h" + +using std::invalid_argument; +using std::min; +using std::numeric_limits; +using std::size_t; +using std::string; +using std::vector; + +using serial::bytesize_t; +using serial::flowcontrol_t; +using serial::IOException; +using serial::parity_t; +using serial::Serial; +using serial::SerialException; +using serial::stopbits_t; + +class Serial::ScopedReadLock { +public: + ScopedReadLock(SerialImpl *pimpl) : pimpl_(pimpl) { + this->pimpl_->readLock(); + } + ~ScopedReadLock() { this->pimpl_->readUnlock(); } + +private: + // Disable copy constructors + ScopedReadLock(const ScopedReadLock &); + const ScopedReadLock &operator=(ScopedReadLock); + + SerialImpl *pimpl_; +}; + +class Serial::ScopedWriteLock { +public: + ScopedWriteLock(SerialImpl *pimpl) : pimpl_(pimpl) { + this->pimpl_->writeLock(); + } + ~ScopedWriteLock() { this->pimpl_->writeUnlock(); } + +private: + // Disable copy constructors there + ScopedWriteLock(const ScopedWriteLock &); + const ScopedWriteLock &operator=(ScopedWriteLock); + SerialImpl *pimpl_; +}; + +Serial::Serial(const string &port, uint32_t baudrate, serial::Timeout timeout, + bytesize_t bytesize, parity_t parity, stopbits_t stopbits, + flowcontrol_t flowcontrol) + : pimpl_(new SerialImpl(port, baudrate, bytesize, parity, stopbits, + flowcontrol)) { + pimpl_->setTimeout(timeout); +} + +Serial::~Serial() { delete pimpl_; } + +void Serial::open() { pimpl_->open(); } + +void Serial::close() { pimpl_->close(); } + +bool Serial::isOpen() const { return pimpl_->isOpen(); } + +size_t Serial::available() { return pimpl_->available(); } + +bool Serial::waitReadable() { + serial::Timeout timeout(pimpl_->getTimeout()); + return pimpl_->waitReadable(timeout.read_timeout_constant); +} + +void Serial::waitByteTimes(size_t count) { pimpl_->waitByteTimes(count); } + +size_t Serial::read_(uint8_t *buffer, size_t size) { + return this->pimpl_->read(buffer, size); +} + +size_t Serial::read(uint8_t *buffer, size_t size) { + ScopedReadLock lock(this->pimpl_); + return this->pimpl_->read(buffer, size); +} + +size_t Serial::read(std::vector &buffer, size_t size) { + ScopedReadLock lock(this->pimpl_); + uint8_t *buffer_ = new uint8_t[size]; + size_t bytes_read = 0; + + try { + bytes_read = this->pimpl_->read(buffer_, size); + } catch (const std::exception &e) { + delete[] buffer_; + throw; + } + + buffer.insert(buffer.end(), buffer_, buffer_ + bytes_read); + delete[] buffer_; + return bytes_read; +} + +size_t Serial::read(std::string &buffer, size_t size) { + ScopedReadLock lock(this->pimpl_); + uint8_t *buffer_ = new uint8_t[size]; + size_t bytes_read = 0; + try { + bytes_read = this->pimpl_->read(buffer_, size); + } catch (const std::exception &e) { + delete[] buffer_; + throw; + } + buffer.append(reinterpret_cast(buffer_), bytes_read); + delete[] buffer_; + return bytes_read; +} + +string Serial::read(size_t size) { + std::string buffer; + this->read(buffer, size); + return buffer; +} + +size_t Serial::readline(string &buffer, size_t size, string eol) { + ScopedReadLock lock(this->pimpl_); + size_t eol_len = eol.length(); + uint8_t *buffer_ = static_cast(alloca(size * sizeof(uint8_t))); + size_t read_so_far = 0; + while (true) { + size_t bytes_read = this->read_(buffer_ + read_so_far, 1); + read_so_far += bytes_read; + if (bytes_read == 0) { + break; // Timeout occured on reading 1 byte + } + if (read_so_far < eol_len) + continue; + if (string(reinterpret_cast(buffer_ + read_so_far - eol_len), + eol_len) == eol) { + break; // EOL found + } + if (read_so_far == size) { + break; // Reached the maximum read length + } + } + buffer.append(reinterpret_cast(buffer_), read_so_far); + return read_so_far; +} + +string Serial::readline(size_t size, string eol) { + std::string buffer; + this->readline(buffer, size, eol); + return buffer; +} + +vector Serial::readlines(size_t size, string eol) { + ScopedReadLock lock(this->pimpl_); + std::vector lines; + size_t eol_len = eol.length(); + uint8_t *buffer_ = static_cast(alloca(size * sizeof(uint8_t))); + size_t read_so_far = 0; + size_t start_of_line = 0; + while (read_so_far < size) { + size_t bytes_read = this->read_(buffer_ + read_so_far, 1); + read_so_far += bytes_read; + if (bytes_read == 0) { + if (start_of_line != read_so_far) { + lines.push_back( + string(reinterpret_cast(buffer_ + start_of_line), + read_so_far - start_of_line)); + } + break; // Timeout occured on reading 1 byte + } + if (read_so_far < eol_len) + continue; + if (string(reinterpret_cast(buffer_ + read_so_far - eol_len), + eol_len) == eol) { + // EOL found + lines.push_back( + string(reinterpret_cast(buffer_ + start_of_line), + read_so_far - start_of_line)); + start_of_line = read_so_far; + } + if (read_so_far == size) { + if (start_of_line != read_so_far) { + lines.push_back( + string(reinterpret_cast(buffer_ + start_of_line), + read_so_far - start_of_line)); + } + break; // Reached the maximum read length + } + } + return lines; +} + +size_t Serial::write(const string &data) { + ScopedWriteLock lock(this->pimpl_); + return this->write_(reinterpret_cast(data.c_str()), + data.length()); +} + +size_t Serial::write(const std::vector &data) { + ScopedWriteLock lock(this->pimpl_); + return this->write_(&data[0], data.size()); +} + +size_t Serial::write(const uint8_t *data, size_t size) { + ScopedWriteLock lock(this->pimpl_); + return this->write_(data, size); +} + +size_t Serial::write_(const uint8_t *data, size_t length) { + return pimpl_->write(data, length); +} + +void Serial::setPort(const string &port) { + ScopedReadLock rlock(this->pimpl_); + ScopedWriteLock wlock(this->pimpl_); + bool was_open = pimpl_->isOpen(); + if (was_open) + close(); + pimpl_->setPort(port); + if (was_open) + open(); +} + +string Serial::getPort() const { return pimpl_->getPort(); } + +void Serial::setTimeout(serial::Timeout &timeout) { + pimpl_->setTimeout(timeout); +} + +serial::Timeout Serial::getTimeout() const { return pimpl_->getTimeout(); } + +void Serial::setBaudrate(uint32_t baudrate) { pimpl_->setBaudrate(baudrate); } + +uint32_t Serial::getBaudrate() const { return uint32_t(pimpl_->getBaudrate()); } + +void Serial::setBytesize(bytesize_t bytesize) { pimpl_->setBytesize(bytesize); } + +bytesize_t Serial::getBytesize() const { return pimpl_->getBytesize(); } + +void Serial::setParity(parity_t parity) { pimpl_->setParity(parity); } + +parity_t Serial::getParity() const { return pimpl_->getParity(); } + +void Serial::setStopbits(stopbits_t stopbits) { pimpl_->setStopbits(stopbits); } + +stopbits_t Serial::getStopbits() const { return pimpl_->getStopbits(); } + +void Serial::setFlowcontrol(flowcontrol_t flowcontrol) { + pimpl_->setFlowcontrol(flowcontrol); +} + +flowcontrol_t Serial::getFlowcontrol() const { + return pimpl_->getFlowcontrol(); +} + +void Serial::flush() { + ScopedReadLock rlock(this->pimpl_); + ScopedWriteLock wlock(this->pimpl_); + pimpl_->flush(); +} + +void Serial::flushInput() { + ScopedReadLock lock(this->pimpl_); + pimpl_->flushInput(); +} + +void Serial::flushOutput() { + ScopedWriteLock lock(this->pimpl_); + pimpl_->flushOutput(); +} + +void Serial::sendBreak(int duration) { pimpl_->sendBreak(duration); } + +void Serial::setBreak(bool level) { pimpl_->setBreak(level); } + +void Serial::setRTS(bool level) { pimpl_->setRTS(level); } + +void Serial::setDTR(bool level) { pimpl_->setDTR(level); } + +bool Serial::waitForChange() { return pimpl_->waitForChange(); } + +bool Serial::getCTS() { return pimpl_->getCTS(); } + +bool Serial::getDSR() { return pimpl_->getDSR(); } + +bool Serial::getRI() { return pimpl_->getRI(); } + +bool Serial::getCD() { return pimpl_->getCD(); } From 16db2ead30fe3a6362b43b865005cda89be5b084 Mon Sep 17 00:00:00 2001 From: laptoy <2107884590@qq.com> Date: Fri, 7 Jun 2024 21:44:41 +0800 Subject: [PATCH 5/9] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/GPSManager.cpp | 55 ++++++++---- src/gimbal_server.cpp | 77 ++++++++++++++--- src/serial.cpp | 197 ++++++++++++++++++++++++++++++------------ 3 files changed, 243 insertions(+), 86 deletions(-) diff --git a/src/GPSManager.cpp b/src/GPSManager.cpp index 4c90fea..9faacb9 100644 --- a/src/GPSManager.cpp +++ b/src/GPSManager.cpp @@ -7,36 +7,45 @@ #include "SettingsManager.h" #include "RTKSettings.h" +// GPSManager类的构造函数，它是一个工具类，用于管理GPS连接和数据处理。 GPSManager::GPSManager(QGCApplication* app, QGCToolbox* toolbox) - : QGCTool(app, toolbox) + : QGCTool(app, toolbox) // 调用基类QGCTool的构造函数 { + // 注册元类型，以便在信号和槽之间传递GPSPositionMessage和GPSSatelliteMessage类型的对象 qRegisterMetaType(); qRegisterMetaType(); } +// GPSManager类的析构函数 GPSManager::~GPSManager() { + // 断开与GPS的连接 disconnectGPS(); } +// 连接到指定的GPS设备 void GPSManager::connectGPS(const QString& device, const QString& gps_type) { + // 获取RTK设置实例，用于访问RTK相关的配置参数 RTKSettings* rtkSettings = qgcApp()->toolbox()->settingsManager()->rtkSettings(); + // 根据传入的gps_type确定GPS设备的类型 GPSProvider::GPSType type; if (gps_type.contains("trimble", Qt::CaseInsensitive)) { type = GPSProvider::GPSType::trimble; - qCDebug(RTKGPSLog) << "Connecting Trimble device"; + qCDebug(RTKGPSLog) << "Connecting Trimble device"; // 打印连接Trimble设备的调试信息 } else if (gps_type.contains("septentrio", Qt::CaseInsensitive)) { type = GPSProvider::GPSType::septentrio; - qCDebug(RTKGPSLog) << "Connecting Septentrio device"; + qCDebug(RTKGPSLog) << "Connecting Septentrio device"; // 打印连接Septentrio设备的调试信息 } else { type = GPSProvider::GPSType::u_blox; - qCDebug(RTKGPSLog) << "Connecting U-blox device"; + qCDebug(RTKGPSLog) << "Connecting U-blox device"; // 打印连接U-blox设备的调试信息 } + // 断开现有的GPS连接 disconnectGPS(); - _requestGpsStop = false; + _requestGpsStop = false; // 清除停止GPS请求的标志 + // 创建一个新的GPSProvider实例，并传入设备名称和类型以及相关的RTK设置 _gpsProvider = new GPSProvider(device, type, true, /* enableSatInfo */ @@ -48,46 +57,58 @@ void GPSManager::connectGPS(const QString& device, const QString& gps_type) rtkSettings->fixedBasePositionAltitude()->rawValue().toFloat(), rtkSettings->fixedBasePositionAccuracy()->rawValue().toFloat(), _requestGpsStop); - _gpsProvider->start(); + _gpsProvider->start(); // 启动GPSProvider - //create RTCM device + // 创建RTCM MAVLink设备 _rtcmMavlink = new RTCMMavlink(*_toolbox); + // 连接GPSProvider的RTCM数据更新信号到RTCMMavlink的槽 connect(_gpsProvider, &GPSProvider::RTCMDataUpdate, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate); - //test: connect to position update + // 连接GPSProvider的位置更新信号到GPSManager的位置更新槽 connect(_gpsProvider, &GPSProvider::positionUpdate, this, &GPSManager::GPSPositionUpdate); + // 连接GPSProvider的卫星信息更新信号到GPSManager的卫星信息更新槽 connect(_gpsProvider, &GPSProvider::satelliteInfoUpdate, this, &GPSManager::GPSSatelliteUpdate); + // 连接GPSProvider的完成信号到GPSManager的断开连接槽 connect(_gpsProvider, &GPSProvider::finished, this, &GPSManager::onDisconnect); + // 连接GPSProvider的测量状态信号到GPSManager的测量状态槽 connect(_gpsProvider, &GPSProvider::surveyInStatus, this, &GPSManager::surveyInStatus); + // 发送连接事件 emit onConnect(); } + +// 断开与GPS的连接 void GPSManager::disconnectGPS(void) { if (_gpsProvider) { - _requestGpsStop = true; - //Note that we need a relatively high timeout to be sure the GPS thread finished. + _requestGpsStop = true; // 设置请求停止GPS的标志 + // 注意，我们需要一个相对较高的超时时间，以确保GPS线程完成。 if (!_gpsProvider->wait(2000)) { - qWarning() << "Failed to wait for GPS thread exit. Consider increasing the timeout"; + qWarning() << "Failed to wait for GPS thread exit. Consider increasing the timeout"; // 如果等待失败，打印警告信息 } - delete(_gpsProvider); + delete(_gpsProvider); // 删除GPSProvider对象 } if (_rtcmMavlink) { - delete(_rtcmMavlink); + delete(_rtcmMavlink); // 删除RTCMMavlink对象 } - _gpsProvider = nullptr; - _rtcmMavlink = nullptr; + _gpsProvider = nullptr; // 将GPSProvider指针设置为nullptr + _rtcmMavlink = nullptr; // 将RTCMMavlink指针设置为nullptr } - +// 当接收到GPS位置更新时调用此函数 void GPSManager::GPSPositionUpdate(GPSPositionMessage msg) { qCDebug(RTKGPSLog) << QString("GPS: got position update: alt=%1, long=%2, lat=%3").arg(msg.position_data.alt).arg(msg.position_data.lon).arg(msg.position_data.lat); + // 打印接收到的GPS位置信息 } + +// 当接收到GPS卫星信息更新时调用此函数 void GPSManager::GPSSatelliteUpdate(GPSSatelliteMessage msg) { qCDebug(RTKGPSLog) << QString("GPS: got satellite info update, %1 satellites").arg((int)msg.satellite_data.count); - emit satelliteUpdate(msg.satellite_data.count); + // 打印接收到的GPS卫星数量信息 + emit satelliteUpdate(msg.satellite_data.count); // 发送卫星更新信号 } + diff --git a/src/gimbal_server.cpp b/src/gimbal_server.cpp index 39c1fde..c8d9f4a 100644 --- a/src/gimbal_server.cpp +++ b/src/gimbal_server.cpp @@ -16,30 +16,40 @@ #include // amovGimbal::gimbal *gimbal = nullptr; +// 定义一个全局的GimbalBase指针，用于后续对云台的控制 GimbalBase *g_gimbal = nullptr; +// 定义一个全局的GimbalState对象，可能用于发布云台的状态信息，但在这段代码中并未使用 prometheus_gimbal_control::GimbalState g_pub_info; + +// 定义一个全局布尔变量，用于控制是否停止追踪（可能是某种目标追踪功能） bool g_stop_track = false; +// 回调函数，当接收到GimbalControl消息时触发 void ctlCallback(const prometheus_gimbal_control::GimbalControl::ConstPtr &ctl) { + // 根据GimbalControl消息中的rpyMode字段判断并执行相应的云台控制操作 switch (ctl->rpyMode) { case prometheus_gimbal_control::GimbalControl::manual: + // 定义两个数组，一个用于存储角度控制（roll, yaw, pitch），一个用于存储速度值 uint8_t rpy_ctl[3]; double rpy_vel[3]; + + // 从GimbalControl消息中提取roll, yaw, pitch的角度值 rpy_ctl[0] = ctl->roll; rpy_ctl[1] = ctl->yaw; rpy_ctl[2] = ctl->pitch; + + // 从GimbalControl消息中提取rValue, pValue, yValue的速度值 rpy_vel[0] = ctl->rValue; rpy_vel[1] = ctl->pValue; rpy_vel[2] = ctl->yValue; - - + // 调用g_gimbal对象的ctl_gimbal方法，传入角度和速度值来控制云台 g_gimbal->ctl_gimbal(rpy_ctl, rpy_vel); - + // 打印日志，显示当前处于手动模式，并显示角度和速度值 ROS_INFO_STREAM("Manual " << "rpy control mode: " << (int)rpy_ctl[0] << " " << (int)rpy_ctl[1] << " " << (int)rpy_ctl[2] << " value: " << rpy_vel[0] << " " << rpy_vel[1] << " " << rpy_vel[2]); @@ -91,49 +101,67 @@ void ctlCallback(const prometheus_gimbal_control::GimbalControl::ConstPtr &ctl) } }; +// 这个回调函数用于处理视觉差分信息，并根据这些信息控制云台追踪目标。 void trackCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr ¢) { + // 如果已经设置了停止追踪的标志，则直接返回，不执行追踪。 if (g_stop_track) return; + + // 定义一个临时数组，用于存储目标中心点的坐标偏移量。 float tmp_cent[2]{0, 0}; + // 计算目标中心点在图像中的x坐标偏移量。 tmp_cent[0] = cent->objectX + cent->objectWidth / 2 - cent->frameWidth / 2; + // 计算目标中心点在图像中的y坐标偏移量。 tmp_cent[1] = cent->objectY + cent->objectHeight / 2 - cent->frameHeight / 2; + // 获取云台的滚动模式。 int roll_mode = cent->ctlMode; + // 获取PID控制参数：比例系数kp。 float kp = cent->kp; + // 获取PID控制参数：积分系数ki。 float ki = cent->ki; + // 获取PID控制参数：微分系数kd。 float kd = cent->kd; + // 定义静态变量，用于记录是否进行了变焦操作。 static bool flag_zoom = false; + // 定义静态变量，用于记录是否进行了保持操作。 static bool flag_hold = false; + // 如果检测到目标。 if (cent->detect != 0) { + // 根据当前大小和最大变焦倍数计算变焦值。 int zoom_val = cent->currSize ? std::fmax(g_gimbal->zoom_val, 1) : 1; + // 根据变焦值计算缩放比例。 float scale = std::fmax(g_gimbal->max_zoom - zoom_val, 1.) / g_gimbal->max_zoom; + // 使用计算出的参数调用云台追踪函数。 g_gimbal->tracking(tmp_cent, roll_mode, kp * scale, ki, kd, cent->trackIgnoreError * zoom_val / g_gimbal->max_zoom); - // std::cout << "step 3 : " << std::endl; + // 如果设置了自动变焦，并且目标在可忽略误差范围内，则进行自动变焦。 if (cent->autoZoom && cent->currSize && tmp_cent[0] < cent->trackIgnoreError && tmp_cent[1] < cent->trackIgnoreError) { g_gimbal->auto_zoom(cent->currSize, zoom_val, cent->maxSize, cent->minSize); flag_zoom = true; } - // std::cout << "step 4 : " << std::endl; + // 设置保持标志为真。 flag_hold = true; } else { + // 如果检测不到目标，并且之前进行了变焦操作，则停止变焦。 if (cent->currSize && flag_zoom) { g_gimbal->zoom_stop(); flag_zoom = false; } + // 如果之前进行了保持操作，则调用云台保持函数。 if (flag_hold) { g_gimbal->hold(); flag_hold = false; } } -}; +} void currSizeCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr &info) { @@ -155,23 +183,32 @@ void currSizeCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr &inf } } +// 这个函数是一个服务回调函数，用于处理视频录制的开始和停止请求。 bool videoRecord(std_srvs::SetBool::Request &ref, std_srvs::SetBool::Response &req) { + // 如果请求的内容是开始录制视频。 if (ref.data) { + // 调用云台对象的开始录制视频函数。 g_gimbal->start_record(); + // 设置响应的成功标志为视频流是否已成功打开。 req.success = g_gimbal->outvideo.isOpened(); + // 打印信息到ROS日志。 ROS_INFO_STREAM("Start Video Record ! "); } else { + // 如果请求的内容是停止录制视频。 + // 如果视频流没有打开，则直接返回失败响应。 if (!g_gimbal->outvideo.isOpened()) { req.success = false; req.message = "video record not is started ! "; ROS_WARN_STREAM("video record not is started ! "); } + // 调用云台对象的完成录制视频函数。 g_gimbal->finish_record(); + // 如果视频流仍然打开，说明停止录制失败。 if (g_gimbal->outvideo.isOpened()) { req.success = false; @@ -180,83 +217,97 @@ bool videoRecord(std_srvs::SetBool::Request &ref, std_srvs::SetBool::Response &r } else { + // 如果视频流已关闭，说明停止录制成功。 req.success = true; ROS_INFO_STREAM("Finish Video Record ! "); } } + // 返回true表示处理成功。 return true; } + void picCallback(const std_msgs::String::ConstPtr &info) { ROS_INFO("Get Picture "); g_gimbal->get_picture(); } +// 这个函数用于连续从云台获取图像帧，并通过图像发布器发布。 void callAll(image_transport::Publisher *image_pub) { cv::Mat frame; while (true) { + // 从云台获取一帧图像。 (*g_gimbal) >> frame; + // 如果帧不为空，则将其发布。 if (!frame.empty()) { image_pub->publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg()); } + // 等待10毫秒。 std::this_thread::sleep_for(10ms); } } +// 这个服务回调函数用于启动或停止搜索模式。 bool search(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp) { + // 设置响应的成功标志为云台搜索操作的结果。 resp.success = g_gimbal->search(req.data); return true; } -// 切换反馈模式，自动？交换 +// 这个服务回调函数用于切换跟踪模式，以获取真实的角速度或计算出的角速度。 bool switchTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp) { + // 将云台的获取真实角速度标志设置为请求中的数据。 static_cast(g_gimbal)->isGetRealVelocity = req.data; resp.success = true; + // 设置响应消息，表明是返回真实角速度还是计算角速度。 resp.message = static_cast(g_gimbal)->isGetRealVelocity ? "return real angle speed" : "return compute angle speed"; return true; } -// 屏蔽tracking控制 +// 这个服务回调函数用于停止跟踪控制。 bool stopTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp) { + // 设置全局停止跟踪标志为请求中的数据。 g_stop_track = req.data; resp.success = true; return true; } +// 这个函数是设备状态回调函数，用于处理设备的不同状态信息。 int VLK_DevStatusCallback(int iType, const char* szBuffer, int iBufLen, void* pUserParam) { + // 根据状态类型处理不同的信息。 if (VLK_DEV_STATUS_TYPE_MODEL == iType) { + // 如果是设备模型信息，则解析并打印模型代码和名称。 VLK_DEV_MODEL* pModel = (VLK_DEV_MODEL*)szBuffer; std::cout << "model code: " << pModel->cModelCode << ", model name: " << pModel->szModelName << std::endl; } else if (VLK_DEV_STATUS_TYPE_CONFIG == iType) { + // 如果是设备配置信息，则解析并打印版本号、设备ID和序列号。 VLK_DEV_CONFIG* pDevConfig = (VLK_DEV_CONFIG*)szBuffer; std::cout << "VersionNO: " << pDevConfig->cVersionNO << ", DeviceID: " << pDevConfig->cDeviceID << ", SerialNO: " << pDevConfig->cSerialNO << std::endl; } else if (VLK_DEV_STATUS_TYPE_TELEMETRY == iType) { - /* - * once device is connected, telemetry information will keep updating, - * in order to avoid disturbing user input, comment out printing telemetry information - */ + // 如果是设备遥测信息，则解析并打印偏航角、俯仰角、传感器类型和变焦倍数。 + // 为了避免干扰用户输入，注释掉了打印遥测信息的代码。 // VLK_DEV_TELEMETRY* pTelemetry = (VLK_DEV_TELEMETRY*)szBuffer; // cout << "Yaw: " << pTelemetry->dYaw << ", Pitch: " << pTelemetry->dPitch << ", sensor type: " << pTelemetry->emSensorType << ", Zoom mag times: " << pTelemetry->sZoomMagTimes << endl; } else { + // 如果是未知的状态类型，则打印错误信息。 std::cout << "error: unknown status type: " << iType << std::endl; } - return 0; } diff --git a/src/serial.cpp b/src/serial.cpp index a1dae49..57f17ce 100644 --- a/src/serial.cpp +++ b/src/serial.cpp @@ -25,173 +25,258 @@ using serial::stopbits_t; class Serial::ScopedReadLock { public: + // 构造函数，传入串行实现类指针，并获取读锁 ScopedReadLock(SerialImpl *pimpl) : pimpl_(pimpl) { this->pimpl_->readLock(); } + // 析构函数，释放读锁 ~ScopedReadLock() { this->pimpl_->readUnlock(); } private: - // Disable copy constructors + // 禁用复制构造函数 ScopedReadLock(const ScopedReadLock &); + // 禁用赋值操作符 const ScopedReadLock &operator=(ScopedReadLock); SerialImpl *pimpl_; }; -class Serial::ScopedWriteLock { -public: - ScopedWriteLock(SerialImpl *pimpl) : pimpl_(pimpl) { - this->pimpl_->writeLock(); - } - ~ScopedWriteLock() { this->pimpl_->writeUnlock(); } - +// 这是一个私有的类定义部分，可能是一个互斥锁封装类 +class ScopedWriteLock { private: - // Disable copy constructors there - ScopedWriteLock(const ScopedWriteLock &); - const ScopedWriteLock &operator=(ScopedWriteLock); + // 禁用拷贝构造函数，避免对象被误拷贝 + // Disable copy constructors there + ScopedWriteLock(const ScopedWriteLock &); // 删除此函数或声明为=delete + + // 禁用赋值运算符，避免对象被误赋值 + const ScopedWriteLock &operator=(ScopedWriteLock); // 缺少引用符号，应改为const ScopedWriteLock& operator=(const ScopedWriteLock&) = delete; + + // 指向实现类的指针，使用了Pimpl（Pointer to Implementation）技术 SerialImpl *pimpl_; }; -Serial::Serial(const string &port, uint32_t baudrate, serial::Timeout timeout, - bytesize_t bytesize, parity_t parity, stopbits_t stopbits, - flowcontrol_t flowcontrol) - : pimpl_(new SerialImpl(port, baudrate, bytesize, parity, stopbits, - flowcontrol)) { - pimpl_->setTimeout(timeout); -} +// Serial类定义，可能是一个串口通信类 +class Serial { +public: + // 构造函数，初始化串口设置 + Serial(const string &port, uint32_t baudrate, serial::Timeout timeout, + bytesize_t bytesize, parity_t parity, stopbits_t stopbits, + flowcontrol_t flowcontrol) + : pimpl_(new SerialImpl(port, baudrate, bytesize, parity, stopbits, + flowcontrol)) { + pimpl_->setTimeout(timeout); // 设置超时时间 + } -Serial::~Serial() { delete pimpl_; } + // 析构函数，释放资源 + ~Serial() { delete pimpl_; } -void Serial::open() { pimpl_->open(); } + // 打开串口 + void open() { pimpl_->open(); } -void Serial::close() { pimpl_->close(); } + // 关闭串口 + void close() { pimpl_->close(); } -bool Serial::isOpen() const { return pimpl_->isOpen(); } + // 检查串口是否打开 + bool isOpen() const { return pimpl_->isOpen(); } -size_t Serial::available() { return pimpl_->available(); } + // 获取串口上可用的字节数 + size_t available() { return pimpl_->available(); } -bool Serial::waitReadable() { - serial::Timeout timeout(pimpl_->getTimeout()); - return pimpl_->waitReadable(timeout.read_timeout_constant); -} + // 等待串口可读 + bool waitReadable() { + serial::Timeout timeout(pimpl_->getTimeout()); + return pimpl_->waitReadable(timeout.read_timeout_constant); + } -void Serial::waitByteTimes(size_t count) { pimpl_->waitByteTimes(count); } + // 等待特定数量的字节时间（可能是为了同步） + void waitByteTimes(size_t count) { pimpl_->waitByteTimes(count); } -size_t Serial::read_(uint8_t *buffer, size_t size) { - return this->pimpl_->read(buffer, size); -} + // 从串口读取数据（无锁） + size_t read_(uint8_t *buffer, size_t size) { + return this->pimpl_->read(buffer, size); + } -size_t Serial::read(uint8_t *buffer, size_t size) { - ScopedReadLock lock(this->pimpl_); - return this->pimpl_->read(buffer, size); -} + // 从串口读取数据（使用ScopedReadLock加锁） + size_t read(uint8_t *buffer, size_t size) { + ScopedReadLock lock(this->pimpl_); // 假设ScopedReadLock是一个读取锁类 + return this->pimpl_->read(buffer, size); + } + +private: + // 指向实现类的指针，使用了Pimpl（Pointer to Implementation）技术 + SerialImpl *pimpl_; +}; size_t Serial::read(std::vector &buffer, size_t size) { + // 创建一个读锁，保证读操作的安全性 ScopedReadLock lock(this->pimpl_); + // 分配一个新的缓冲区来存储读取的数据 uint8_t *buffer_ = new uint8_t[size]; + // 初始化已读取的字节数为0 size_t bytes_read = 0; try { + // 尝试从串行端口读取数据到缓冲区 bytes_read = this->pimpl_->read(buffer_, size); } catch (const std::exception &e) { + // 如果读取过程中发生异常，删除缓冲区并重新抛出异常 delete[] buffer_; throw; } + // 将读取的数据插入到传入的vector中 buffer.insert(buffer.end(), buffer_, buffer_ + bytes_read); + // 删除临时缓冲区 delete[] buffer_; + // 返回实际读取的字节数 return bytes_read; } size_t Serial::read(std::string &buffer, size_t size) { + // 创建一个读锁，保证读操作的安全性 ScopedReadLock lock(this->pimpl_); + // 分配一个新的缓冲区来存储读取的数据 uint8_t *buffer_ = new uint8_t[size]; + // 初始化已读取的字节数为0 size_t bytes_read = 0; try { + // 尝试从串行端口读取数据到缓冲区 bytes_read = this->pimpl_->read(buffer_, size); } catch (const std::exception &e) { + // 如果读取过程中发生异常，删除缓冲区并重新抛出异常 delete[] buffer_; throw; } + // 将读取的数据追加到传入的string中 buffer.append(reinterpret_cast(buffer_), bytes_read); + // 删除临时缓冲区 delete[] buffer_; + // 返回实际读取的字节数 return bytes_read; } + +// Serial类中的成员函数，用于读取指定字节数并返回字符串 string Serial::read(size_t size) { - std::string buffer; - this->read(buffer, size); - return buffer; + std::string buffer; // 创建一个字符串用于存储读取的数据 + buffer.resize(size); // 预分配足够的存储空间 + this->read(&buffer[0], size); // 调用重载的read函数，将读取的数据填充到buffer中 + return buffer; // 返回读取到的字符串 } +// Serial类中的成员函数，用于读取一行数据并返回字符串 size_t Serial::readline(string &buffer, size_t size, string eol) { - ScopedReadLock lock(this->pimpl_); - size_t eol_len = eol.length(); + ScopedReadLock lock(this->pimpl_); // 使用ScopedReadLock对读取操作进行加锁 + size_t eol_len = eol.length(); // 获取行结束符的长度 + // 使用alloca函数在栈上动态分配足够大小的字节数组来存储读取的数据 + // 注意：alloca函数不是C++标准库的一部分，并且在某些平台上可能不被支持或存在安全问题 + // 在实践中，更推荐使用std::vector或std::array来替代 uint8_t *buffer_ = static_cast(alloca(size * sizeof(uint8_t))); - size_t read_so_far = 0; + size_t read_so_far = 0; // 记录已经读取的字节数 while (true) { + // 尝试读取一个字节 size_t bytes_read = this->read_(buffer_ + read_so_far, 1); read_so_far += bytes_read; + // 如果没有读取到任何数据（可能是超时），则退出循环 if (bytes_read == 0) { - break; // Timeout occured on reading 1 byte + break; // 读取1字节时发生超时 } + // 如果已经读取的字节数还不足以构成一个完整的行结束符，则继续读取 if (read_so_far < eol_len) continue; + // 检查当前读取到的数据是否包含行结束符 if (string(reinterpret_cast(buffer_ + read_so_far - eol_len), eol_len) == eol) { - break; // EOL found + break; // 找到行结束符 } + // 如果已经读取了最大字节数，则退出循环 if (read_so_far == size) { - break; // Reached the maximum read length + break; // 达到最大读取长度 } } + // 将读取到的数据转换为字符串并追加到传入的buffer中 buffer.append(reinterpret_cast(buffer_), read_so_far); + // 返回实际读取的字节数 return read_so_far; } +// Serial类中的成员函数，读取一行数据并返回字符串（使用临时字符串对象） string Serial::readline(size_t size, string eol) { - std::string buffer; - this->readline(buffer, size, eol); - return buffer; + std::string buffer; // 创建一个临时字符串对象 + this->readline(buffer, size, eol); // 调用重载的readline函数 + return buffer; // 返回读取到的字符串 } +// 中文注释： +// Serial::read函数： +// 读取指定字节数的数据，并将数据转换为字符串返回。 +// +// Serial::readline函数（带引用参数）： +// 读取一行数据，直到遇到指定的行结束符或者读取了指定数量的字节或者发生超时。 +// 读取到的数据会被追加到传入的字符串buffer中，并返回实际读取的字节数。 +// 该函数使用ScopedReadLock进行加锁操作，以确保在多线程环境中线程安全。 +// +// Serial::readline函数（返回字符串）： +// 与带引用参数的readline函数功能相同，但返回的是一个新创建的字符串对象。 +// 内部使用了一个临时字符串对象来存储读取到的数据。 + +// Serial类中的成员函数，用于从串口读取多行数据并返回一个string向量 vector Serial::readlines(size_t size, string eol) { - ScopedReadLock lock(this->pimpl_); - std::vector lines; - size_t eol_len = eol.length(); + ScopedReadLock lock(this->pimpl_); // 使用ScopedReadLock对读取操作进行加锁 + std::vector lines; // 创建一个字符串向量来存储读取到的多行数据 + size_t eol_len = eol.length(); // 获取行结束符的长度 + + // 注意：虽然这里使用了alloca来分配内存，但在C++中更推荐使用std::vector或std::array + // 这里为了保持与原始代码的一致性，仍然使用alloca uint8_t *buffer_ = static_cast(alloca(size * sizeof(uint8_t))); - size_t read_so_far = 0; - size_t start_of_line = 0; + size_t read_so_far = 0; // 记录已经读取的字节数 + size_t start_of_line = 0; // 记录当前行的起始位置 + + // 循环读取数据，直到达到最大读取长度或发生超时 while (read_so_far < size) { + // 尝试读取一个字节 size_t bytes_read = this->read_(buffer_ + read_so_far, 1); read_so_far += bytes_read; + + // 如果读取失败（可能是超时） if (bytes_read == 0) { + // 如果当前行有数据（即start_of_line不等于当前位置），则添加到lines中 if (start_of_line != read_so_far) { lines.push_back( string(reinterpret_cast(buffer_ + start_of_line), read_so_far - start_of_line)); } - break; // Timeout occured on reading 1 byte + break; // 读取1字节时发生超时，退出循环 } + + // 如果当前读取的字节数还不足以构成一个完整的行结束符，则继续读取 if (read_so_far < eol_len) continue; + + // 检查当前读取到的数据是否包含行结束符 if (string(reinterpret_cast(buffer_ + read_so_far - eol_len), eol_len) == eol) { - // EOL found + // 发现行结束符，将当前行添加到lines中 lines.push_back( string(reinterpret_cast(buffer_ + start_of_line), - read_so_far - start_of_line)); + read_so_far - eol_len)); + // 更新当前行的起始位置为行结束符之后 start_of_line = read_so_far; } + + // 如果已经读取了最大字节数，则退出循环 if (read_so_far == size) { + // 如果当前行有数据（即start_of_line不等于当前位置），则添加到lines中 if (start_of_line != read_so_far) { lines.push_back( string(reinterpret_cast(buffer_ + start_of_line), read_so_far - start_of_line)); } - break; // Reached the maximum read length + break; // 达到最大读取长度，退出循环 } } + + // 返回读取到的多行数据 return lines; } From 9f3203ed4da168342839cf2044e73a68bb5c3a47 Mon Sep 17 00:00:00 2001 From: laptoy <2107884590@qq.com> Date: Tue, 11 Jun 2024 14:53:16 +0800 Subject: [PATCH 6/9] add --- src/Camera/QGCCameraControl.cc | 2259 ++++++++++++++++++++++++++++++++ src/Camera/QGCCameraControl.h | 422 ++++++ src/Camera/QGCCameraIO.cc | 372 ++++++ src/Camera/QGCCameraIO.h | 72 + src/Camera/QGCCameraManager.cc | 518 ++++++++ src/Camera/QGCCameraManager.h | 112 ++ README.md => src/README.md | 0 src/api/QGCCorePlugin.cc | 502 +++++++ src/api/QGCCorePlugin.h | 222 ++++ src/api/QGCOptions.cc | 47 + src/api/QGCOptions.h | 170 +++ src/api/QGCSettings.cc | 21 + src/api/QGCSettings.h | 37 + src/api/QmlComponentInfo.cc | 19 + src/api/QmlComponentInfo.h | 35 + 15 files changed, 4808 insertions(+) create mode 100644 src/Camera/QGCCameraControl.cc create mode 100644 src/Camera/QGCCameraControl.h create mode 100644 src/Camera/QGCCameraIO.cc create mode 100644 src/Camera/QGCCameraIO.h create mode 100644 src/Camera/QGCCameraManager.cc create mode 100644 src/Camera/QGCCameraManager.h rename README.md => src/README.md (100%) create mode 100644 src/api/QGCCorePlugin.cc create mode 100644 src/api/QGCCorePlugin.h create mode 100644 src/api/QGCOptions.cc create mode 100644 src/api/QGCOptions.h create mode 100644 src/api/QGCSettings.cc create mode 100644 src/api/QGCSettings.h create mode 100644 src/api/QmlComponentInfo.cc create mode 100644 src/api/QmlComponentInfo.h diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc new file mode 100644 index 0000000..f85acb7 --- /dev/null +++ b/src/Camera/QGCCameraControl.cc @@ -0,0 +1,2259 @@ + + +#include "QGCCameraControl.h" +#include "QGCCameraIO.h" +#include "SettingsManager.h" +#include "VideoManager.h" +#include "QGCMapEngine.h" +#include "QGCCameraManager.h" +#include "FTPManager.h" +#include "QGCLZMA.h" + +#include +#include +#include +#include + +QGC_LOGGING_CATEGORY(CameraControlLog, "CameraControlLog") +QGC_LOGGING_CATEGORY(CameraControlVerboseLog, "CameraControlVerboseLog") + +static const char* kCondition = "condition"; +static const char* kControl = "control"; +static const char* kDefault = "default"; +static const char* kDefnition = "definition"; +static const char* kDescription = "description"; +static const char* kExclusion = "exclude"; +static const char* kExclusions = "exclusions"; +static const char* kLocale = "locale"; +static const char* kLocalization = "localization"; +static const char* kMax = "max"; +static const char* kMin = "min"; +static const char* kModel = "model"; +static const char* kName = "name"; +static const char* kOption = "option"; +static const char* kOptions = "options"; +static const char* kOriginal = "original"; +static const char* kParameter = "parameter"; +static const char* kParameterrange = "parameterrange"; +static const char* kParameterranges = "parameterranges"; +static const char* kParameters = "parameters"; +static const char* kReadOnly = "readonly"; +static const char* kWriteOnly = "writeonly"; +static const char* kRoption = "roption"; +static const char* kStep = "step"; +static const char* kDecimalPlaces = "decimalPlaces"; +static const char* kStrings = "strings"; +static const char* kTranslated = "translated"; +static const char* kType = "type"; +static const char* kUnit = "unit"; +static const char* kUpdate = "update"; +static const char* kUpdates = "updates"; +static const char* kValue = "value"; +static const char* kVendor = "vendor"; +static const char* kVersion = "version"; + +static const char* kPhotoMode = "PhotoMode"; +static const char* kPhotoLapse = "PhotoLapse"; +static const char* kPhotoLapseCount = "PhotoLapseCount"; +static const char* kThermalOpacity = "ThermalOpacity"; +static const char* kThermalMode = "ThermalMode"; + +//----------------------------------------------------------------------------- +// Known Parameters +const char* QGCCameraControl::kCAM_EV = "CAM_EV"; +const char* QGCCameraControl::kCAM_EXPMODE = "CAM_EXPMODE"; +const char* QGCCameraControl::kCAM_ISO = "CAM_ISO"; +const char* QGCCameraControl::kCAM_SHUTTERSPD = "CAM_SHUTTERSPD"; +const char* QGCCameraControl::kCAM_APERTURE = "CAM_APERTURE"; +const char* QGCCameraControl::kCAM_WBMODE = "CAM_WBMODE"; +const char* QGCCameraControl::kCAM_MODE = "CAM_MODE"; + +//----------------------------------------------------------------------------- +QGCCameraOptionExclusion::QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_) + : QObject(parent) + , param(param_) + , value(value_) + , exclusions(exclusions_) +{ +} + +//----------------------------------------------------------------------------- +QGCCameraOptionRange::QGCCameraOptionRange(QObject* parent, QString param_, QString value_, QString targetParam_, QString condition_, QStringList optNames_, QStringList optValues_) + : QObject(parent) + , param(param_) + , value(value_) + , targetParam(targetParam_) + , condition(condition_) + , optNames(optNames_) + , optValues(optValues_) +{ +} + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, bool& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue() != "0"; + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, int& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue().toInt(); + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, QString& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue(); + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_value(QDomNode& element, const char* tagName, QString& target) +{ + QDomElement de = element.firstChildElement(tagName); + if(de.isNull()) { + return false; + } + target = de.text(); + return true; +} + +//----------------------------------------------------------------------------- +QGCCameraControl::QGCCameraControl(const mavlink_camera_information_t *info, Vehicle* vehicle, int compID, QObject* parent) + : FactGroup(0, parent, true /* ignore camel case */) + , _vehicle(vehicle) + , _compID(compID) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + memcpy(&_info, info, sizeof(mavlink_camera_information_t)); + connect(this, &QGCCameraControl::dataReady, this, &QGCCameraControl::_dataReady); + _vendor = QString(reinterpret_cast(info->vendor_name)); + _modelName = QString(reinterpret_cast(info->model_name)); + int ver = static_cast(_info.cam_definition_version); + _cacheFile = QString::asprintf("%s/%s_%s_%03d.xml", + qgcApp()->toolbox()->settingsManager()->appSettings()->parameterSavePath().toStdString().c_str(), + _vendor.toStdString().c_str(), + _modelName.toStdString().c_str(), + ver); + if(info->cam_definition_uri[0] != 0) { + //-- Process camera definition file + _handleDefinitionFile(info->cam_definition_uri); + } else { + _initWhenReady(); + } + QSettings settings; + _photoMode = static_cast(settings.value(kPhotoMode, static_cast(PHOTO_CAPTURE_SINGLE)).toInt()); + _photoLapse = settings.value(kPhotoLapse, 1.0).toDouble(); + _photoLapseCount = settings.value(kPhotoLapseCount, 0).toInt(); + _thermalOpacity = settings.value(kThermalOpacity, 85.0).toDouble(); + _thermalMode = static_cast(settings.value(kThermalMode, static_cast(THERMAL_BLEND)).toUInt()); + _recTimer.setSingleShot(false); + _recTimer.setInterval(333); + connect(&_recTimer, &QTimer::timeout, this, &QGCCameraControl::_recTimerHandler); +} + +//----------------------------------------------------------------------------- +QGCCameraControl::~QGCCameraControl() +{ + delete _netManager; + _netManager = nullptr; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_initWhenReady() +{ + qCDebug(CameraControlLog) << "_initWhenReady()"; + if(isBasic()) { + qCDebug(CameraControlLog) << "Basic, MAVLink only messages."; + _requestCameraSettings(); + QTimer::singleShot(250, this, &QGCCameraControl::_checkForVideoStreams); + //-- Basic cameras have no parameters + _paramComplete = true; + emit parametersReady(); + } else { + _requestAllParameters(); + //-- Give some time to load the parameters before going after the camera settings + QTimer::singleShot(2000, this, &QGCCameraControl::_requestCameraSettings); + } + connect(_vehicle, &Vehicle::mavCommandResult, this, &QGCCameraControl::_mavCommandResult); + connect(&_captureStatusTimer, &QTimer::timeout, this, &QGCCameraControl::_requestCaptureStatus); + _captureStatusTimer.setSingleShot(true); + QTimer::singleShot(2500, this, &QGCCameraControl::_requestStorageInfo); + _captureStatusTimer.start(2750); + emit infoChanged(); + + delete _netManager; + _netManager = nullptr; +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::firmwareVersion() +{ + int major = (_info.firmware_version >> 24) & 0xFF; + int minor = (_info.firmware_version >> 16) & 0xFF; + int build = _info.firmware_version & 0xFFFF; + return QString::asprintf("%d.%d.%d", major, minor, build); +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::recordTimeStr() +{ + return QTime(0, 0).addMSecs(static_cast(recordTime())).toString("hh:mm:ss"); +} + +//----------------------------------------------------------------------------- +QGCCameraControl::VideoStatus +QGCCameraControl::videoStatus() +{ + return _video_status; +} + +//----------------------------------------------------------------------------- +QGCCameraControl::PhotoStatus +QGCCameraControl::photoStatus() +{ + return _photo_status; +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::storageFreeStr() +{ + return QGCMapEngine::storageFreeSizeToString(static_cast(_storageFree)); +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::batteryRemainingStr() +{ + if(_batteryRemaining >= 0) { + return QGCMapEngine::numberToString(static_cast(_batteryRemaining)) + " %"; + } + return ""; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setCameraMode(CameraMode mode) +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "setCameraMode(" << mode << ")"; + if(mode == CAM_MODE_VIDEO) { + setVideoMode(); + } else if(mode == CAM_MODE_PHOTO) { + setPhotoMode(); + } else { + qCDebug(CameraControlLog) << "setCameraMode() Invalid mode:" << mode; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoMode(PhotoMode mode) +{ + if(!_resetting) { + _photoMode = mode; + QSettings settings; + settings.setValue(kPhotoMode, static_cast(mode)); + emit photoModeChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoLapse(qreal interval) +{ + _photoLapse = interval; + QSettings settings; + settings.setValue(kPhotoLapse, interval); + emit photoLapseChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoLapseCount(int count) +{ + _photoLapseCount = count; + QSettings settings; + settings.setValue(kPhotoLapseCount, count); + emit photoLapseCountChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_setCameraMode(CameraMode mode) +{ + if(_cameraMode != mode) { + _cameraMode = mode; + emit cameraModeChanged(); + //-- Update stream status + _streamStatusTimer.start(1000); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::toggleMode() +{ + if(!_resetting) { + if(cameraMode() == CAM_MODE_PHOTO || cameraMode() == CAM_MODE_SURVEY) { + setVideoMode(); + } else if(cameraMode() == CAM_MODE_VIDEO) { + setPhotoMode(); + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::toggleVideo() +{ + if(!_resetting) { + if(videoStatus() == VIDEO_CAPTURE_STATUS_RUNNING) { + return stopVideo(); + } else { + return startVideo(); + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::takePhoto() +{ + qCDebug(CameraControlLog) << "takePhoto()"; + //-- Check if camera can capture photos or if it can capture it while in Video Mode + if(!capturesPhotos()) { + qCWarning(CameraControlLog) << "Camera does not handle image capture"; + return false; + } + if(cameraMode() == CAM_MODE_VIDEO && !photosInVideoMode()) { + qCWarning(CameraControlLog) << "Camera does not handle image capture while in video mode"; + return false; + } + if(photoStatus() != PHOTO_CAPTURE_IDLE) { + qCWarning(CameraControlLog) << "Camera not idle"; + return false; + } + if(!_resetting) { + if(capturesPhotos()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_IMAGE_START_CAPTURE, // Command id + false, // ShowError + 0, // Reserved (Set to 0) + static_cast(_photoMode == PHOTO_CAPTURE_SINGLE ? 0 : _photoLapse), // Duration between two consecutive pictures (in seconds--ignored if single image) + _photoMode == PHOTO_CAPTURE_SINGLE ? 1 : _photoLapseCount); // Number of images to capture total - 0 for unlimited capture + _setPhotoStatus(PHOTO_CAPTURE_IN_PROGRESS); + _captureInfoRetries = 0; + //-- Capture local image as well + if(qgcApp()->toolbox()->videoManager()) { + qgcApp()->toolbox()->videoManager()->grabImage(); + } + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::stopTakePhoto() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "stopTakePhoto()"; + if(photoStatus() == PHOTO_CAPTURE_IDLE || (photoStatus() != PHOTO_CAPTURE_INTERVAL_IDLE && photoStatus() != PHOTO_CAPTURE_INTERVAL_IN_PROGRESS)) { + return false; + } + if(capturesPhotos()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_IMAGE_STOP_CAPTURE, // Command id + false, // ShowError + 0); // Reserved (Set to 0) + _setPhotoStatus(PHOTO_CAPTURE_IDLE); + _captureInfoRetries = 0; + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::startVideo() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "startVideo()"; + //-- Check if camera can capture videos or if it can capture it while in Photo Mode + if(!capturesVideo() || (cameraMode() == CAM_MODE_PHOTO && !videoInPhotoMode())) { + return false; + } + if(videoStatus() != VIDEO_CAPTURE_STATUS_RUNNING) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_START_CAPTURE, // Command id + false, // Don't Show Error (handle locally) + 0, // Reserved (Set to 0) + 0); // CAMERA_CAPTURE_STATUS Frequency + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::stopVideo() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "stopVideo()"; + if(videoStatus() == VIDEO_CAPTURE_STATUS_RUNNING) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_STOP_CAPTURE, // Command id + false, // Don't Show Error (handle locally) + 0); // Reserved (Set to 0) + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setVideoMode() +{ + if(!_resetting && hasModes()) { + qCDebug(CameraControlLog) << "setVideoMode()"; + //-- Does it have a mode parameter? + Fact* pMode = mode(); + if(pMode) { + if(cameraMode() != CAM_MODE_VIDEO) { + pMode->setRawValue(CAM_MODE_VIDEO); + _setCameraMode(CAM_MODE_VIDEO); + } + } else { + //-- Use MAVLink Command + if(_cameraMode != CAM_MODE_VIDEO) { + //-- Use basic MAVLink message + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_MODE, // Command id + true, // ShowError + 0, // Reserved (Set to 0) + CAM_MODE_VIDEO); // Camera mode (0: photo, 1: video) + _setCameraMode(CAM_MODE_VIDEO); + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoMode() +{ + if(!_resetting && hasModes()) { + qCDebug(CameraControlLog) << "setPhotoMode()"; + //-- Does it have a mode parameter? + Fact* pMode = mode(); + if(pMode) { + if(cameraMode() != CAM_MODE_PHOTO) { + pMode->setRawValue(CAM_MODE_PHOTO); + _setCameraMode(CAM_MODE_PHOTO); + } + } else { + //-- Use MAVLink Command + if(_cameraMode != CAM_MODE_PHOTO) { + //-- Use basic MAVLink message + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_MODE, // Command id + true, // ShowError + 0, // Reserved (Set to 0) + CAM_MODE_PHOTO); // Camera mode (0: photo, 1: video) + _setCameraMode(CAM_MODE_PHOTO); + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setThermalMode(ThermalViewMode mode) +{ + QSettings settings; + settings.setValue(kThermalMode, static_cast(mode)); + _thermalMode = mode; + emit thermalModeChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setThermalOpacity(double val) +{ + if(val < 0.0) val = 0.0; + if(val > 100.0) val = 100.0; + if(fabs(_thermalOpacity - val) > 0.1) { + _thermalOpacity = val; + QSettings settings; + settings.setValue(kThermalOpacity, val); + emit thermalOpacityChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setZoomLevel(qreal level) +{ + qCDebug(CameraControlLog) << "setZoomLevel()" << level; + if(hasZoom()) { + //-- Limit + level = std::min(std::max(level, 0.0), 100.0); + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_RANGE, // Zoom type + static_cast(level)); // Level + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setFocusLevel(qreal level) +{ + qCDebug(CameraControlLog) << "setFocusLevel()" << level; + if(hasFocus()) { + //-- Limit + level = std::min(std::max(level, 0.0), 100.0); + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_FOCUS, // Command id + false, // ShowError + FOCUS_TYPE_RANGE, // Focus type + static_cast(level)); // Level + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::resetSettings() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "resetSettings()"; + _resetting = true; + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_RESET_CAMERA_SETTINGS, // Command id + true, // ShowError + 1); // Do Reset + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::formatCard(int id) +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "formatCard()"; + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_STORAGE_FORMAT, // Command id + true, // ShowError + id, // Storage ID (1 for first, 2 for second, etc.) + 1); // Do Format + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::stepZoom(int direction) +{ + qCDebug(CameraControlLog) << "stepZoom()" << direction; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_STEP, // Zoom type + direction); // Direction (-1 wide, 1 tele) + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::startZoom(int direction) +{ + qCDebug(CameraControlLog) << "startZoom()" << direction; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_CONTINUOUS, // Zoom type + direction); // Direction (-1 wide, 1 tele) + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::stopZoom() +{ + qCDebug(CameraControlLog) << "stopZoom()"; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_CONTINUOUS, // Zoom type + 0); // Direction (-1 wide, 1 tele) + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestCaptureStatus() +{ + qCDebug(CameraControlLog) << "_requestCaptureStatus()"; + _vehicle->sendMavCommand( + _compID, // target component + MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS, // command id + false, // showError + 1); // Do Request +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::factChanged(Fact* pFact) +{ + _updateActiveList(); + _updateRanges(pFact); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle) +{ + //-- Is this ours? + if(_vehicle->id() != vehicleId || compID() != component) { + return; + } + if(!noReponseFromVehicle && result == MAV_RESULT_IN_PROGRESS) { + //-- Do Nothing + qCDebug(CameraControlLog) << "In progress response for" << command; + }else if(!noReponseFromVehicle && result == MAV_RESULT_ACCEPTED) { + switch(command) { + case MAV_CMD_RESET_CAMERA_SETTINGS: + _resetting = false; + if(isBasic()) { + _requestCameraSettings(); + } else { + QTimer::singleShot(500, this, &QGCCameraControl::_requestAllParameters); + QTimer::singleShot(2500, this, &QGCCameraControl::_requestCameraSettings); + } + break; + case MAV_CMD_VIDEO_START_CAPTURE: + _setVideoStatus(VIDEO_CAPTURE_STATUS_RUNNING); + _captureStatusTimer.start(1000); + break; + case MAV_CMD_VIDEO_STOP_CAPTURE: + _setVideoStatus(VIDEO_CAPTURE_STATUS_STOPPED); + _captureStatusTimer.start(1000); + break; + case MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS: + _captureInfoRetries = 0; + break; + case MAV_CMD_REQUEST_STORAGE_INFORMATION: + _storageInfoRetries = 0; + break; + case MAV_CMD_IMAGE_START_CAPTURE: + _captureStatusTimer.start(1000); + break; + } + } else { + if(noReponseFromVehicle || result == MAV_RESULT_TEMPORARILY_REJECTED || result == MAV_RESULT_FAILED) { + if(noReponseFromVehicle) { + qCDebug(CameraControlLog) << "No response for" << command; + } else if (result == MAV_RESULT_TEMPORARILY_REJECTED) { + qCDebug(CameraControlLog) << "Command temporarily rejected for" << command; + } else { + qCDebug(CameraControlLog) << "Command failed for" << command; + } + switch(command) { + case MAV_CMD_RESET_CAMERA_SETTINGS: + _resetting = false; + qCDebug(CameraControlLog) << "Failed to reset camera settings"; + break; + case MAV_CMD_IMAGE_START_CAPTURE: + case MAV_CMD_IMAGE_STOP_CAPTURE: + if(++_captureInfoRetries < 3) { + _captureStatusTimer.start(1000); + } else { + qCDebug(CameraControlLog) << "Giving up start/stop image capture"; + _setPhotoStatus(PHOTO_CAPTURE_IDLE); + } + break; + case MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS: + if(++_captureInfoRetries < 3) { + _captureStatusTimer.start(500); + } else { + qCDebug(CameraControlLog) << "Giving up requesting capture status"; + } + break; + case MAV_CMD_REQUEST_STORAGE_INFORMATION: + if(++_storageInfoRetries < 3) { + QTimer::singleShot(500, this, &QGCCameraControl::_requestStorageInfo); + } else { + qCDebug(CameraControlLog) << "Giving up requesting storage status"; + } + break; + } + } else { + qCDebug(CameraControlLog) << "Bad response for" << command << result; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_setVideoStatus(VideoStatus status) +{ + if(_video_status != status) { + _video_status = status; + emit videoStatusChanged(); + if(status == VIDEO_CAPTURE_STATUS_RUNNING) { + _recordTime = 0; + _recTime = QTime::currentTime(); + _recTimer.start(); + } else { + _recTimer.stop(); + _recordTime = 0; + emit recordTimeChanged(); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_recTimerHandler() +{ + _recordTime = static_cast(_recTime.msecsTo(QTime::currentTime())); + emit recordTimeChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_setPhotoStatus(PhotoStatus status) +{ + if(_photo_status != status) { + qCDebug(CameraControlLog) << "Set Photo Status:" << status; + _photo_status = status; + emit photoStatusChanged(); + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadCameraDefinitionFile(QByteArray& bytes) +{ + QByteArray originalData(bytes); + //-- Handle localization + if(!_handleLocalization(bytes)) { + return false; + } + int errorLine; + QString errorMsg; + QDomDocument doc; + if(!doc.setContent(bytes, false, &errorMsg, &errorLine)) { + qCCritical(CameraControlLog) << "Unable to parse camera definition file on line:" << errorLine; + qCCritical(CameraControlLog) << errorMsg; + return false; + } + //-- Load camera constants + QDomNodeList defElements = doc.elementsByTagName(kDefnition); + if(!defElements.size() || !_loadConstants(defElements)) { + qCWarning(CameraControlLog) << "Unable to load camera constants from camera definition"; + return false; + } + //-- Load camera parameters + QDomNodeList paramElements = doc.elementsByTagName(kParameters); + if(!paramElements.size()) { + qCDebug(CameraControlLog) << "No parameters to load from camera"; + return false; + } + if(!_loadSettings(paramElements)) { + qCWarning(CameraControlLog) << "Unable to load camera parameters from camera definition"; + return false; + } + //-- If this is new, cache it + if(!_cached) { + qCDebug(CameraControlLog) << "Saving camera definition file" << _cacheFile; + QFile file(_cacheFile); + if (!file.open(QIODevice::WriteOnly)) { + qWarning() << QString("Could not save cache file %1. Error: %2").arg(_cacheFile).arg(file.errorString()); + } else { + file.write(originalData); + } + } + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadConstants(const QDomNodeList nodeList) +{ + QDomNode node = nodeList.item(0); + if(!read_attribute(node, kVersion, _version)) { + return false; + } + if(!read_value(node, kModel, _modelName)) { + return false; + } + if(!read_value(node, kVendor, _vendor)) { + return false; + } + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadSettings(const QDomNodeList nodeList) +{ + QDomNode node = nodeList.item(0); + QDomElement elem = node.toElement(); + QDomNodeList parameters = elem.elementsByTagName(kParameter); + //-- Pre-process settings (maintain order and skip non-controls) + for(int i = 0; i < parameters.size(); i++) { + QDomNode parameterNode = parameters.item(i); + QString name; + if(read_attribute(parameterNode, kName, name)) { + bool control = true; + read_attribute(parameterNode, kControl, control); + if(control) { + _settings << name; + } + } else { + qCritical() << "Parameter entry missing parameter name"; + return false; + } + } + //-- Load parameters + for(int i = 0; i < parameters.size(); i++) { + QDomNode parameterNode = parameters.item(i); + QString factName; + read_attribute(parameterNode, kName, factName); + QString type; + if(!read_attribute(parameterNode, kType, type)) { + qCritical() << QString("Parameter %1 missing parameter type").arg(factName); + return false; + } + //-- Does it have a control? + bool control = true; + read_attribute(parameterNode, kControl, control); + //-- Is it read only? + bool readOnly = false; + read_attribute(parameterNode, kReadOnly, readOnly); + //-- Is it write only? + bool writeOnly = false; + read_attribute(parameterNode, kWriteOnly, writeOnly); + //-- It can't be both + if(readOnly && writeOnly) { + qCritical() << QString("Parameter %1 cannot be both read only and write only").arg(factName); + } + //-- Param type + bool unknownType; + FactMetaData::ValueType_t factType = FactMetaData::stringToType(type, unknownType); + if (unknownType) { + qCritical() << QString("Unknown type for parameter %1").arg(factName); + return false; + } + //-- By definition, custom types do not have control + if(factType == FactMetaData::valueTypeCustom) { + control = false; + } + //-- Description + QString description; + if(!read_value(parameterNode, kDescription, description)) { + qCritical() << QString("Parameter %1 missing parameter description").arg(factName); + return false; + } + //-- Check for updates + QStringList updates = _loadUpdates(parameterNode); + if(updates.size()) { + qCDebug(CameraControlVerboseLog) << "Parameter" << factName << "requires updates for:" << updates; + _requestUpdates[factName] = updates; + } + //-- Build metadata + FactMetaData* metaData = new FactMetaData(factType, factName, this); + QQmlEngine::setObjectOwnership(metaData, QQmlEngine::CppOwnership); + metaData->setShortDescription(description); + metaData->setLongDescription(description); + metaData->setHasControl(control); + metaData->setReadOnly(readOnly); + metaData->setWriteOnly(writeOnly); + //-- Options (enums) + QDomElement optionElem = parameterNode.toElement(); + QDomNodeList optionsRoot = optionElem.elementsByTagName(kOptions); + if(optionsRoot.size()) { + //-- Iterate options + QDomNode node = optionsRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList options = elem.elementsByTagName(kOption); + for(int i = 0; i < options.size(); i++) { + QDomNode option = options.item(i); + QString optName; + QString optValue; + QVariant optVariant; + if(!_loadNameValue(option, factName, metaData, optName, optValue, optVariant)) { + delete metaData; + return false; + } + metaData->addEnumInfo(optName, optVariant); + _originalOptNames[factName] << optName; + _originalOptValues[factName] << optVariant; + //-- Check for exclusions + QStringList exclusions = _loadExclusions(option); + if(exclusions.size()) { + qCDebug(CameraControlVerboseLog) << "New exclusions:" << factName << optValue << exclusions; + QGCCameraOptionExclusion* pExc = new QGCCameraOptionExclusion(this, factName, optValue, exclusions); + QQmlEngine::setObjectOwnership(pExc, QQmlEngine::CppOwnership); + _valueExclusions.append(pExc); + } + //-- Check for range rules + if(!_loadRanges(option, factName, optValue)) { + delete metaData; + return false; + } + } + } + QString defaultValue; + if(read_attribute(parameterNode, kDefault, defaultValue)) { + QVariant defaultVariant; + QString errorString; + if (metaData->convertAndValidateRaw(defaultValue, false, defaultVariant, errorString)) { + metaData->setRawDefaultValue(defaultVariant); + } else { + qWarning() << "Invalid default value for" << factName + << " type:" << metaData->type() + << " value:" << defaultValue + << " error:" << errorString; + } + } + //-- Set metadata and Fact + if (_nameToFactMetaDataMap.contains(factName)) { + qWarning() << QStringLiteral("Duplicate fact name:") << factName; + delete metaData; + } else { + { + //-- Check for Min Value + QString attr; + if(read_attribute(parameterNode, kMin, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setRawMin(typedValue); + } else { + qWarning() << "Invalid min value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Max Value + QString attr; + if(read_attribute(parameterNode, kMax, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setRawMax(typedValue); + } else { + qWarning() << "Invalid max value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Step Value + QString attr; + if(read_attribute(parameterNode, kStep, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setRawIncrement(typedValue.toDouble()); + } else { + qWarning() << "Invalid step value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Decimal Places + QString attr; + if(read_attribute(parameterNode, kDecimalPlaces, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setDecimalPlaces(typedValue.toInt()); + } else { + qWarning() << "Invalid decimal places value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Units + QString attr; + if(read_attribute(parameterNode, kUnit, attr)) { + metaData->setRawUnits(attr); + } + } + qCDebug(CameraControlLog) << "New parameter:" << factName << (readOnly ? "ReadOnly" : "Writable") << (writeOnly ? "WriteOnly" : "Readable"); + _nameToFactMetaDataMap[factName] = metaData; + Fact* pFact = new Fact(_compID, factName, factType, this); + QQmlEngine::setObjectOwnership(pFact, QQmlEngine::CppOwnership); + pFact->setMetaData(metaData); + pFact->_containerSetRawValue(metaData->rawDefaultValue()); + QGCCameraParamIO* pIO = new QGCCameraParamIO(this, pFact, _vehicle); + QQmlEngine::setObjectOwnership(pIO, QQmlEngine::CppOwnership); + _paramIO[factName] = pIO; + _addFact(pFact, factName); + } + } + if(_nameToFactMetaDataMap.size() > 0) { + _addFactGroup(this, "camera"); + _processRanges(); + _activeSettings = _settings; + emit activeSettingsChanged(); + return true; + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_handleLocalization(QByteArray& bytes) +{ + QString errorMsg; + int errorLine; + QDomDocument doc; + if(!doc.setContent(bytes, false, &errorMsg, &errorLine)) { + qCritical() << "Unable to parse camera definition file on line:" << errorLine; + qCritical() << errorMsg; + return false; + } + //-- Find out where we are + QLocale locale = QLocale::system(); +#if defined (Q_OS_MAC) + locale = QLocale(locale.name()); +#endif + QString localeName = locale.name().toLower().replace("-", "_"); + qCDebug(CameraControlLog) << "Current locale:" << localeName; + if(localeName == "en_us") { + // Nothing to do + return true; + } + QDomNodeList locRoot = doc.elementsByTagName(kLocalization); + if(!locRoot.size()) { + // Nothing to do + return true; + } + //-- Iterate locales + QDomNode node = locRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList locales = elem.elementsByTagName(kLocale); + for(int i = 0; i < locales.size(); i++) { + QDomNode locale = locales.item(i); + QString name; + if(!read_attribute(locale, kName, name)) { + qWarning() << "Localization entry is missing its name attribute"; + continue; + } + // If we found a direct match, deal with it now + if(localeName == name.toLower().replace("-", "_")) { + return _replaceLocaleStrings(locale, bytes); + } + } + //-- No direct match. Pick first matching language (if any) + localeName = localeName.left(3); + for(int i = 0; i < locales.size(); i++) { + QDomNode locale = locales.item(i); + QString name; + read_attribute(locale, kName, name); + if(name.toLower().startsWith(localeName)) { + return _replaceLocaleStrings(locale, bytes); + } + } + //-- Could not find a language to use + qWarning() << "No match for" << QLocale::system().name() << "in camera definition file"; + //-- Just use default, en_US + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_replaceLocaleStrings(const QDomNode node, QByteArray& bytes) +{ + QDomElement stringElem = node.toElement(); + QDomNodeList strings = stringElem.elementsByTagName(kStrings); + for(int i = 0; i < strings.size(); i++) { + QDomNode stringNode = strings.item(i); + QString original; + QString translated; + if(read_attribute(stringNode, kOriginal, original)) { + if(read_attribute(stringNode, kTranslated, translated)) { + QString o; o = "\"" + original + "\""; + QString t; t = "\"" + translated + "\""; + bytes.replace(o.toUtf8(), t.toUtf8()); + o = ">" + original + "<"; + t = ">" + translated + "<"; + bytes.replace(o.toUtf8(), t.toUtf8()); + } + } + } + return true; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestAllParameters() +{ + //-- Reset receive list + for(const QString& paramName: _paramIO.keys()) { + if(_paramIO[paramName]) { + _paramIO[paramName]->setParamRequest(); + } else { + qCritical() << "QGCParamIO is NULL" << paramName; + } + } + WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); + if (!weakLink.expired()) { + SharedLinkInterfacePtr sharedLink = weakLink.lock(); + + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_message_t msg; + mavlink_msg_param_ext_request_list_pack_chan( + static_cast(mavlink->getSystemId()), + static_cast(mavlink->getComponentId()), + sharedLink->mavlinkChannel(), + &msg, + static_cast(_vehicle->id()), + static_cast(compID())); + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } + qCDebug(CameraControlVerboseLog) << "Request all parameters"; +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::_getParamName(const char* param_id) +{ + QByteArray bytes(param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + QString parameterName(bytes); + return parameterName; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleParamAck(const mavlink_param_ext_ack_t& ack) +{ + QString paramName = _getParamName(ack.param_id); + if(!_paramIO.contains(paramName)) { + qCWarning(CameraControlLog) << "Received PARAM_EXT_ACK for unknown param:" << paramName; + return; + } + if(_paramIO[paramName]) { + _paramIO[paramName]->handleParamAck(ack); + } else { + qCritical() << "QGCParamIO is NULL" << paramName; + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleParamValue(const mavlink_param_ext_value_t& value) +{ + QString paramName = _getParamName(value.param_id); + if(!_paramIO.contains(paramName)) { + qCWarning(CameraControlLog) << "Received PARAM_EXT_VALUE for unknown param:" << paramName; + return; + } + if(_paramIO[paramName]) { + _paramIO[paramName]->handleParamValue(value); + } else { + qCritical() << "QGCParamIO is NULL" << paramName; + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_updateActiveList() +{ + //-- Clear out excluded parameters based on exclusion rules + QStringList exclusionList; + for(QGCCameraOptionExclusion* param: _valueExclusions) { + Fact* pFact = getFact(param->param); + if(pFact) { + QString option = pFact->rawValueString(); + if(param->value == option) { + exclusionList << param->exclusions; + } + } + } + QStringList active; + for(QString key: _settings) { + if(!exclusionList.contains(key)) { + active.append(key); + } + } + if(active != _activeSettings) { + qCDebug(CameraControlVerboseLog) << "Excluding" << exclusionList; + _activeSettings = active; + emit activeSettingsChanged(); + //-- Force validity of "Facts" based on active set + if(_paramComplete) { + emit parametersReady(); + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_processConditionTest(const QString conditionTest) +{ + enum { + TEST_NONE, + TEST_EQUAL, + TEST_NOT_EQUAL, + TEST_GREATER, + TEST_SMALLER + }; + qCDebug(CameraControlVerboseLog) << "_processConditionTest(" << conditionTest << ")"; + int op = TEST_NONE; + QStringList test; + + auto split = [&conditionTest](const QString& sep ) { + return conditionTest.split(sep, Qt::SkipEmptyParts); + }; + + if(conditionTest.contains("!=")) { + test = split("!="); + op = TEST_NOT_EQUAL; + } else if(conditionTest.contains("=")) { + test = split("="); + op = TEST_EQUAL; + } else if(conditionTest.contains(">")) { + test = split(">"); + op = TEST_GREATER; + } else if(conditionTest.contains("<")) { + test = split("<"); + op = TEST_SMALLER; + } + if(test.size() == 2) { + Fact* pFact = getFact(test[0]); + if(pFact) { + switch(op) { + case TEST_EQUAL: + return pFact->rawValueString() == test[1]; + case TEST_NOT_EQUAL: + return pFact->rawValueString() != test[1]; + case TEST_GREATER: + return pFact->rawValueString() > test[1]; + case TEST_SMALLER: + return pFact->rawValueString() < test[1]; + case TEST_NONE: + break; + } + } else { + qWarning() << "Invalid condition parameter:" << test[0] << "in" << conditionTest; + return false; + } + } + qWarning() << "Invalid condition" << conditionTest; + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_processCondition(const QString condition) +{ + qCDebug(CameraControlVerboseLog) << "_processCondition(" << condition << ")"; + bool result = true; + bool andOp = true; + if(!condition.isEmpty()) { + QStringList scond = condition.split(" ", Qt::SkipEmptyParts); + while(scond.size()) { + QString test = scond.first(); + scond.removeFirst(); + if(andOp) { + result = result && _processConditionTest(test); + } else { + result = result || _processConditionTest(test); + } + if(!scond.size()) { + return result; + } + andOp = scond.first().toUpper() == "AND"; + scond.removeFirst(); + } + } + return result; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_updateRanges(Fact* pFact) +{ + QMap rangesSet; + QMap rangesReset; + QStringList changedList; + QStringList resetList; + QStringList updates; + //-- Iterate range sets looking for limited ranges + for(QGCCameraOptionRange* pRange: _optionRanges) { + //-- If this fact or one of its conditions is part of this range set + if(!changedList.contains(pRange->targetParam) && (pRange->param == pFact->name() || pRange->condition.contains(pFact->name()))) { + Fact* pRFact = getFact(pRange->param); //-- This parameter + Fact* pTFact = getFact(pRange->targetParam); //-- The target parameter (the one its range is to change) + if(pRFact && pTFact) { + //qCDebug(CameraControlVerboseLog) << "Check new set of options for" << pTFact->name(); + QString option = pRFact->rawValueString(); //-- This parameter value + //-- If this value (and condition) triggers a change in the target range + //qCDebug(CameraControlVerboseLog) << "Range value:" << pRange->value << "Current value:" << option << "Condition:" << pRange->condition; + if(pRange->value == option && _processCondition(pRange->condition)) { + if(pTFact->enumStrings() != pRange->optNames) { + //-- Set limited range set + rangesSet[pTFact] = pRange; + } + changedList << pRange->targetParam; + } + } + } + } + //-- Iterate range sets again looking for resets + for(QGCCameraOptionRange* pRange: _optionRanges) { + if(!changedList.contains(pRange->targetParam) && (pRange->param == pFact->name() || pRange->condition.contains(pFact->name()))) { + Fact* pTFact = getFact(pRange->targetParam); //-- The target parameter (the one its range is to change) + if(!resetList.contains(pRange->targetParam)) { + if(pTFact->enumStrings() != _originalOptNames[pRange->targetParam]) { + //-- Restore full option set + rangesReset[pTFact] = pRange->targetParam; + } + resetList << pRange->targetParam; + } + } + } + //-- Update limited range set + for (Fact* f: rangesSet.keys()) { + f->setEnumInfo(rangesSet[f]->optNames, rangesSet[f]->optVariants); + if(!updates.contains(f->name())) { + _paramIO[f->name()]->optNames = rangesSet[f]->optNames; + _paramIO[f->name()]->optVariants = rangesSet[f]->optVariants; + emit f->enumsChanged(); + qCDebug(CameraControlVerboseLog) << "Limited set of options for:" << f->name() << rangesSet[f]->optNames;; + updates << f->name(); + } + } + //-- Restore full range set + for (Fact* f: rangesReset.keys()) { + f->setEnumInfo(_originalOptNames[rangesReset[f]], _originalOptValues[rangesReset[f]]); + if(!updates.contains(f->name())) { + _paramIO[f->name()]->optNames = _originalOptNames[rangesReset[f]]; + _paramIO[f->name()]->optVariants = _originalOptValues[rangesReset[f]]; + emit f->enumsChanged(); + qCDebug(CameraControlVerboseLog) << "Restore full set of options for:" << f->name() << _originalOptNames[f->name()]; + updates << f->name(); + } + } + //-- Parameter update requests + if(_requestUpdates.contains(pFact->name())) { + for(const QString& param: _requestUpdates[pFact->name()]) { + if(!_updatesToRequest.contains(param)) { + _updatesToRequest << param; + } + } + } + if(_updatesToRequest.size()) { + QTimer::singleShot(500, this, &QGCCameraControl::_requestParamUpdates); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestParamUpdates() +{ + for(const QString& param: _updatesToRequest) { + _paramIO[param]->paramRequest(); + } + _updatesToRequest.clear(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestCameraSettings() +{ + qCDebug(CameraControlLog) << "_requestCameraSettings()"; + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_CAMERA_SETTINGS, // command id + false, // showError + 1); // Do Request + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestStorageInfo() +{ + qCDebug(CameraControlLog) << "_requestStorageInfo()"; + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_STORAGE_INFORMATION, // command id + false, // showError + 0, // Storage ID (0 for all, 1 for first, 2 for second, etc.) + 1); // Do Request + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleSettings(const mavlink_camera_settings_t& settings) +{ + qCDebug(CameraControlLog) << "handleSettings() Mode:" << settings.mode_id; + _setCameraMode(static_cast(settings.mode_id)); + qreal z = static_cast(settings.zoomLevel); + qreal f = static_cast(settings.focusLevel); + if(std::isfinite(z) && z != _zoomLevel) { + _zoomLevel = z; + emit zoomLevelChanged(); + } + if(std::isfinite(f) && f != _focusLevel) { + _focusLevel = f; + emit focusLevelChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleStorageInfo(const mavlink_storage_information_t& st) +{ + qCDebug(CameraControlLog) << "handleStorageInfo:" << st.available_capacity << st.status << st.storage_count << st.storage_id << st.total_capacity << st.used_capacity; + if(st.status == STORAGE_STATUS_READY) { + uint32_t t = static_cast(st.total_capacity); + if(_storageTotal != t) { + _storageTotal = t; + emit storageTotalChanged(); + } + uint32_t a = static_cast(st.available_capacity); + if(_storageFree != a) { + _storageFree = a; + emit storageFreeChanged(); + } + } + if(_storageStatus != static_cast(st.status)) { + _storageStatus = static_cast(st.status); + emit storageStatusChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleBatteryStatus(const mavlink_battery_status_t& bs) +{ + qCDebug(CameraControlLog) << "handleBatteryStatus:" << bs.battery_remaining; + if(bs.battery_remaining >= 0 && _batteryRemaining != static_cast(bs.battery_remaining)) { + _batteryRemaining = static_cast(bs.battery_remaining); + emit batteryRemainingChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleCaptureStatus(const mavlink_camera_capture_status_t& cap) +{ + //-- This is a response to MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS + qCDebug(CameraControlLog) << "handleCaptureStatus:" << cap.available_capacity << cap.image_interval << cap.image_status << cap.recording_time_ms << cap.video_status; + //-- Disk Free Space + uint32_t a = static_cast(cap.available_capacity); + if(_storageFree != a) { + _storageFree = a; + emit storageFreeChanged(); + } + //-- Do we have recording time? + if(cap.recording_time_ms) { + // Resync our _recTime timer to the time info received from the camera component + _recordTime = cap.recording_time_ms; + _recTime = _recTime.addMSecs(_recTime.msecsTo(QTime::currentTime()) - static_cast(cap.recording_time_ms)); + emit recordTimeChanged(); + } + //-- Video/Image Capture Status + uint8_t vs = cap.video_status < static_cast(VIDEO_CAPTURE_STATUS_LAST) ? cap.video_status : static_cast(VIDEO_CAPTURE_STATUS_UNDEFINED); + uint8_t ps = cap.image_status < static_cast(PHOTO_CAPTURE_LAST) ? cap.image_status : static_cast(PHOTO_CAPTURE_STATUS_UNDEFINED); + _setVideoStatus(static_cast(vs)); + _setPhotoStatus(static_cast(ps)); + //-- Keep asking for it once in a while when recording + if(videoStatus() == VIDEO_CAPTURE_STATUS_RUNNING) { + _captureStatusTimer.start(5000); + //-- Same while (single) image capture is busy + } else if(photoStatus() != PHOTO_CAPTURE_IDLE && photoMode() == PHOTO_CAPTURE_SINGLE) { + _captureStatusTimer.start(1000); + } + //-- Time Lapse + if(photoStatus() == PHOTO_CAPTURE_INTERVAL_IDLE || photoStatus() == PHOTO_CAPTURE_INTERVAL_IN_PROGRESS) { + //-- Capture local image as well + if(qgcApp()->toolbox()->videoManager()) { + QString photoPath = qgcApp()->toolbox()->settingsManager()->appSettings()->savePath()->rawValue().toString() + QStringLiteral("/Photo"); + QDir().mkpath(photoPath); + photoPath += + "/" + QDateTime::currentDateTime().toString("yyyy-MM-dd_hh.mm.ss.zzz") + ".jpg"; + qgcApp()->toolbox()->videoManager()->grabImage(photoPath); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleVideoInfo(const mavlink_video_stream_information_t* vi) +{ + qCDebug(CameraControlLog) << "handleVideoInfo:" << vi->stream_id << vi->uri; + _expectedCount = vi->count; + if(!_findStream(vi->stream_id, false)) { + qCDebug(CameraControlLog) << "Create stream handler for stream ID:" << vi->stream_id; + QGCVideoStreamInfo* pStream = new QGCVideoStreamInfo(this, vi); + QQmlEngine::setObjectOwnership(pStream, QQmlEngine::CppOwnership); + _streams.append(pStream); + //-- Thermal is handled separately and not listed + if(!pStream->isThermal()) { + _streamLabels.append(pStream->name()); + emit streamsChanged(); + emit streamLabelsChanged(); + } else { + emit thermalStreamChanged(); + } + } + //-- Check for missing count + if(_streams.count() < _expectedCount) { + _streamInfoTimer.start(1000); + } else { + //-- Done + qCDebug(CameraControlLog) << "All stream handlers done"; + _streamInfoTimer.stop(); + emit autoStreamChanged(); + emit _vehicle->cameraManager()->streamChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleVideoStatus(const mavlink_video_stream_status_t* vs) +{ + _streamStatusTimer.stop(); + qCDebug(CameraControlLog) << "handleVideoStatus:" << vs->stream_id; + QGCVideoStreamInfo* pInfo = _findStream(vs->stream_id); + if(pInfo) { + pInfo->update(vs); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setCurrentStream(int stream) +{ + if(stream != _currentStream && stream >= 0 && stream < _streamLabels.count()) { + if(_currentStream != stream) { + QGCVideoStreamInfo* pInfo = currentStreamInstance(); + if(pInfo) { + qCDebug(CameraControlLog) << "Stopping stream:" << pInfo->uri(); + //-- Stop current stream + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_STOP_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + } + _currentStream = stream; + pInfo = currentStreamInstance(); + if(pInfo) { + //-- Start new stream + qCDebug(CameraControlLog) << "Starting stream:" << pInfo->uri(); + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_START_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + //-- Update stream status + _requestStreamStatus(static_cast(pInfo->streamID())); + } + emit currentStreamChanged(); + emit _vehicle->cameraManager()->streamChanged(); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::stopStream() +{ + QGCVideoStreamInfo* pInfo = currentStreamInstance(); + if(pInfo) { + //-- Stop current stream + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_STOP_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::resumeStream() +{ + QGCVideoStreamInfo* pInfo = currentStreamInstance(); + if(pInfo) { + //-- Start new stream + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_START_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::autoStream() +{ + if(hasVideoStream()) { + return _streams.count() > 0; + } + return false; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::currentStreamInstance() +{ + if(_currentStream < _streamLabels.count() && _streamLabels.count()) { + QGCVideoStreamInfo* pStream = _findStream(_streamLabels[_currentStream]); + return pStream; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::thermalStreamInstance() +{ + //-- For now, it will return the first thermal listed (if any) + for(int i = 0; i < _streams.count(); i++) { + if(_streams[i]) { + QGCVideoStreamInfo* pStream = qobject_cast(_streams[i]); + if(pStream) { + if(pStream->isThermal()) { + return pStream; + } + } + } + } + return nullptr; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestStreamInfo(uint8_t streamID) +{ + qCDebug(CameraControlLog) << "Requesting video stream info for:" << streamID; + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION, // Command id + false, // ShowError + streamID); // Stream ID +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestStreamStatus(uint8_t streamID) +{ + qCDebug(CameraControlLog) << "Requesting video stream status for:" << streamID; + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_VIDEO_STREAM_STATUS, // Command id + false, // ShowError + streamID); // Stream ID + _streamStatusTimer.start(1000); // Wait up to a second for it +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::_findStream(uint8_t id, bool report) +{ + for(int i = 0; i < _streams.count(); i++) { + if(_streams[i]) { + QGCVideoStreamInfo* pStream = qobject_cast(_streams[i]); + if(pStream) { + if(pStream->streamID() == id) { + return pStream; + } + } else { + qCritical() << "Null QGCVideoStreamInfo instance"; + } + } + } + if(report) { + qWarning() << "Stream id not found:" << id; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::_findStream(const QString name) +{ + for(int i = 0; i < _streams.count(); i++) { + if(_streams[i]) { + QGCVideoStreamInfo* pStream = qobject_cast(_streams[i]); + if(pStream) { + if(pStream->name() == name) { + return pStream; + } + } + } + } + return nullptr; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_streamTimeout() +{ + _requestCount++; + int count = _expectedCount * 3; + if(_requestCount > count) { + qCWarning(CameraControlLog) << "Giving up requesting video stream info"; + _streamInfoTimer.stop(); + //-- If we have at least one stream, work with what we have. + if(_streams.count()) { + emit autoStreamChanged(); + emit _vehicle->cameraManager()->streamChanged(); + } + return; + } + for(uint8_t i = 0; i < _expectedCount; i++) { + //-- Stream ID starts at 1 + if(!_findStream(i+1, false)) { + _requestStreamInfo(i+1); + return; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_streamStatusTimeout() +{ + QGCVideoStreamInfo* pStream = currentStreamInstance(); + if(pStream) { + _requestStreamStatus(static_cast(pStream->streamID())); + } +} + +//----------------------------------------------------------------------------- +QStringList +QGCCameraControl::_loadExclusions(QDomNode option) +{ + QStringList exclusionList; + QDomElement optionElem = option.toElement(); + QDomNodeList excRoot = optionElem.elementsByTagName(kExclusions); + if(excRoot.size()) { + //-- Iterate exclusions + QDomNode node = excRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList exclusions = elem.elementsByTagName(kExclusion); + for(int i = 0; i < exclusions.size(); i++) { + QString exclude = exclusions.item(i).toElement().text(); + if(!exclude.isEmpty()) { + exclusionList << exclude; + } + } + } + return exclusionList; +} + +//----------------------------------------------------------------------------- +QStringList +QGCCameraControl::_loadUpdates(QDomNode option) +{ + QStringList updateList; + QDomElement optionElem = option.toElement(); + QDomNodeList updateRoot = optionElem.elementsByTagName(kUpdates); + if(updateRoot.size()) { + //-- Iterate updates + QDomNode node = updateRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList updates = elem.elementsByTagName(kUpdate); + for(int i = 0; i < updates.size(); i++) { + QString update = updates.item(i).toElement().text(); + if(!update.isEmpty()) { + updateList << update; + } + } + } + return updateList; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadRanges(QDomNode option, const QString factName, QString paramValue) +{ + QDomElement optionElem = option.toElement(); + QDomNodeList rangeRoot = optionElem.elementsByTagName(kParameterranges); + if(rangeRoot.size()) { + QDomNode node = rangeRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList parameterRanges = elem.elementsByTagName(kParameterrange); + //-- Iterate parameter ranges + for(int i = 0; i < parameterRanges.size(); i++) { + QString param; + QString condition; + QDomNode paramRange = parameterRanges.item(i); + if(!read_attribute(paramRange, kParameter, param)) { + qCritical() << QString("Malformed option range for parameter %1").arg(factName); + return false; + } + read_attribute(paramRange, kCondition, condition); + QDomElement pelem = paramRange.toElement(); + QDomNodeList rangeOptions = pelem.elementsByTagName(kRoption); + QStringList optNames; + QStringList optValues; + //-- Iterate options + for(int i = 0; i < rangeOptions.size(); i++) { + QString optName; + QString optValue; + QDomNode roption = rangeOptions.item(i); + if(!read_attribute(roption, kName, optName)) { + qCritical() << QString("Malformed roption for parameter %1").arg(factName); + return false; + } + if(!read_attribute(roption, kValue, optValue)) { + qCritical() << QString("Malformed rvalue for parameter %1").arg(factName); + return false; + } + optNames << optName; + optValues << optValue; + } + if(optNames.size()) { + QGCCameraOptionRange* pRange = new QGCCameraOptionRange(this, factName, paramValue, param, condition, optNames, optValues); + _optionRanges.append(pRange); + qCDebug(CameraControlVerboseLog) << "New range limit:" << factName << paramValue << param << condition << optNames << optValues; + } + } + } + return true; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_processRanges() +{ + //-- After all parameter are loaded, process parameter ranges + for(QGCCameraOptionRange* pRange: _optionRanges) { + Fact* pRFact = getFact(pRange->targetParam); + if(pRFact) { + for(int i = 0; i < pRange->optNames.size(); i++) { + QVariant optVariant; + QString errorString; + if (!pRFact->metaData()->convertAndValidateRaw(pRange->optValues[i], false, optVariant, errorString)) { + qWarning() << "Invalid roption value, name:" << pRange->targetParam + << " type:" << pRFact->metaData()->type() + << " value:" << pRange->optValues[i] + << " error:" << errorString; + } else { + pRange->optVariants << optVariant; + } + } + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadNameValue(QDomNode option, const QString factName, FactMetaData* metaData, QString& optName, QString& optValue, QVariant& optVariant) +{ + if(!read_attribute(option, kName, optName)) { + qCritical() << QString("Malformed option for parameter %1").arg(factName); + return false; + } + if(!read_attribute(option, kValue, optValue)) { + qCritical() << QString("Malformed value for parameter %1").arg(factName); + return false; + } + QString errorString; + if (!metaData->convertAndValidateRaw(optValue, false, optVariant, errorString)) { + qWarning() << "Invalid option value, name:" << factName + << " type:" << metaData->type() + << " value:" << optValue + << " error:" << errorString; + } + return true; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_handleDefinitionFile(const QString &url) +{ + //-- First check and see if we have it cached + QFile xmlFile(_cacheFile); + + QString ftpPrefix(QStringLiteral("%1://").arg(FTPManager::mavlinkFTPScheme)); + if (!xmlFile.exists() && url.startsWith(ftpPrefix, Qt::CaseInsensitive)) { + qCDebug(CameraControlLog) << "No camera definition file cached, attempt ftp download"; + int ver = static_cast(_info.cam_definition_version); + QString ext = ""; + if (url.endsWith(".lzma", Qt::CaseInsensitive)) { ext = ".lzma"; } + if (url.endsWith(".xz", Qt::CaseInsensitive)) { ext = ".xz"; } + QString fileName = QString::asprintf("%s_%s_%03d.xml%s", + _vendor.toStdString().c_str(), + _modelName.toStdString().c_str(), + ver, + ext.toStdString().c_str()); + connect(_vehicle->ftpManager(), &FTPManager::downloadComplete, this, &QGCCameraControl::_ftpDownloadComplete); + _vehicle->ftpManager()->download(_compID, url, + qgcApp()->toolbox()->settingsManager()->appSettings()->parameterSavePath().toStdString().c_str(), + fileName); + return; + } + + if (!xmlFile.exists()) { + qCDebug(CameraControlLog) << "No camera definition file cached, attempt http download"; + _httpRequest(url); + return; + } + if (!xmlFile.open(QIODevice::ReadOnly)) { + qWarning() << "Could not read cached camera definition file:" << _cacheFile; + _httpRequest(url); + return; + } + QByteArray bytes = xmlFile.readAll(); + QDomDocument doc; + if(!doc.setContent(bytes, false)) { + qWarning() << "Could not parse cached camera definition file:" << _cacheFile; + _httpRequest(url); + return; + } + //-- We have it + qCDebug(CameraControlLog) << "Using cached camera definition file:" << _cacheFile; + _cached = true; + emit dataReady(bytes); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_httpRequest(const QString &url) +{ + qCDebug(CameraControlLog) << "Request camera definition:" << url; + if(!_netManager) { + _netManager = new QNetworkAccessManager(this); + } + QNetworkProxy savedProxy = _netManager->proxy(); + QNetworkProxy tempProxy; + tempProxy.setType(QNetworkProxy::DefaultProxy); + _netManager->setProxy(tempProxy); + QNetworkRequest request(url); + request.setAttribute(QNetworkRequest::RedirectPolicyAttribute, true); + QSslConfiguration conf = request.sslConfiguration(); + conf.setPeerVerifyMode(QSslSocket::VerifyNone); + request.setSslConfiguration(conf); + QNetworkReply* reply = _netManager->get(request); + connect(reply, &QNetworkReply::finished, this, &QGCCameraControl::_downloadFinished); + _netManager->setProxy(savedProxy); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_downloadFinished() +{ + QNetworkReply* reply = qobject_cast(sender()); + if(!reply) { + return; + } + int err = reply->error(); + int http_code = reply->attribute(QNetworkRequest::HttpStatusCodeAttribute).toInt(); + QByteArray data = reply->readAll(); + if(err == QNetworkReply::NoError && http_code == 200) { + data.append("\n"); + } else { + data.clear(); + qWarning() << QString("Camera Definition (%1) download error: %2 status: %3").arg( + reply->url().toDisplayString(), + reply->errorString(), + reply->attribute(QNetworkRequest::HttpStatusCodeAttribute).toString() + ); + } + emit dataReady(data); + //reply->deleteLater(); +} + +void QGCCameraControl::_ftpDownloadComplete(const QString& fileName, const QString& errorMsg) +{ + qCDebug(CameraControlLog) << "FTP Download completed: " << fileName << ", " << errorMsg; + + disconnect(_vehicle->ftpManager(), &FTPManager::downloadComplete, this, &QGCCameraControl::_ftpDownloadComplete); + + QString outputFileName = fileName; + + if (fileName.endsWith(".lzma", Qt::CaseInsensitive) || fileName.endsWith(".xz", Qt::CaseInsensitive)) { + outputFileName = fileName.left(fileName.lastIndexOf(".")); + if (QGCLZMA::inflateLZMAFile(fileName, outputFileName)) { + QFile(fileName).remove(); + } else { + qCWarning(CameraControlLog) << "Inflate of compressed xml failed" << fileName; + outputFileName.clear(); + } + } + + QFile xmlFile(outputFileName); + + if (!xmlFile.exists()) { + qCDebug(CameraControlLog) << "No camera definition file present after ftp download completed"; + return; + } + if (!xmlFile.open(QIODevice::ReadOnly)) { + qWarning() << "Could not read downloaded camera definition file: " << fileName; + return; + } + + _cached = true; + QByteArray bytes = xmlFile.readAll(); + emit dataReady(bytes); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_dataReady(QByteArray data) +{ + if(data.size()) { + qCDebug(CameraControlLog) << "Parsing camera definition"; + _loadCameraDefinitionFile(data); + } else { + qCDebug(CameraControlLog) << "No camera definition"; + } + _initWhenReady(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_paramDone() +{ + for(const QString& param: _paramIO.keys()) { + if(!_paramIO[param]->paramDone()) { + return; + } + } + //-- All parameters loaded (or timed out) + _paramComplete = true; + emit parametersReady(); + //-- Check for video streaming + _checkForVideoStreams(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_checkForVideoStreams() +{ + if(_info.flags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM) { + //-- Skip it if using Taisync as it has its own video settings + if(!qgcApp()->toolbox()->videoManager()->isTaisync()) { + connect(&_streamInfoTimer, &QTimer::timeout, this, &QGCCameraControl::_streamTimeout); + _streamInfoTimer.setSingleShot(false); + connect(&_streamStatusTimer, &QTimer::timeout, this, &QGCCameraControl::_streamStatusTimeout); + _streamStatusTimer.setSingleShot(true); + //-- Request all streams + _requestStreamInfo(0); + _streamInfoTimer.start(2000); + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::incomingParameter(Fact* pFact, QVariant& newValue) +{ + Q_UNUSED(pFact); + Q_UNUSED(newValue); + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::validateParameter(Fact* pFact, QVariant& newValue) +{ + Q_UNUSED(pFact); + Q_UNUSED(newValue); + return true; +} + +//----------------------------------------------------------------------------- +QStringList +QGCCameraControl::activeSettings() +{ + qCDebug(CameraControlLog) << "Active:" << _activeSettings; + return _activeSettings; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::exposureMode() +{ + return (_paramComplete && _activeSettings.contains(kCAM_EXPMODE)) ? getFact(kCAM_EXPMODE) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::ev() +{ + return (_paramComplete && _activeSettings.contains(kCAM_EV)) ? getFact(kCAM_EV) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::iso() +{ + return (_paramComplete && _activeSettings.contains(kCAM_ISO)) ? getFact(kCAM_ISO) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::shutterSpeed() +{ + return (_paramComplete && _activeSettings.contains(kCAM_SHUTTERSPD)) ? getFact(kCAM_SHUTTERSPD) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::aperture() +{ + return (_paramComplete && _activeSettings.contains(kCAM_APERTURE)) ? getFact(kCAM_APERTURE) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::wb() +{ + return (_paramComplete && _activeSettings.contains(kCAM_WBMODE)) ? getFact(kCAM_WBMODE) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::mode() +{ + return _paramComplete && factExists(kCAM_MODE) ? getFact(kCAM_MODE) : nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo::QGCVideoStreamInfo(QObject* parent, const mavlink_video_stream_information_t *si) + : QObject(parent) +{ + memcpy(&_streamInfo, si, sizeof(mavlink_video_stream_information_t)); +} + +//----------------------------------------------------------------------------- +qreal +QGCVideoStreamInfo::aspectRatio() const +{ + qreal ar = 1.0; + if(_streamInfo.resolution_h && _streamInfo.resolution_v) { + ar = static_cast(_streamInfo.resolution_h) / static_cast(_streamInfo.resolution_v); + } + return ar; +} + +//----------------------------------------------------------------------------- +bool +QGCVideoStreamInfo::update(const mavlink_video_stream_status_t* vs) +{ + bool changed = false; + if(_streamInfo.hfov != vs->hfov) { + changed = true; + _streamInfo.hfov = vs->hfov; + } + if(_streamInfo.flags != vs->flags) { + changed = true; + _streamInfo.flags = vs->flags; + } + if(_streamInfo.bitrate != vs->bitrate) { + changed = true; + _streamInfo.bitrate = vs->bitrate; + } + if(_streamInfo.rotation != vs->rotation) { + changed = true; + _streamInfo.rotation = vs->rotation; + } + if(_streamInfo.framerate != vs->framerate) { + changed = true; + _streamInfo.framerate = vs->framerate; + } + if(_streamInfo.resolution_h != vs->resolution_h) { + changed = true; + _streamInfo.resolution_h = vs->resolution_h; + } + if(_streamInfo.resolution_v != vs->resolution_v) { + changed = true; + _streamInfo.resolution_v = vs->resolution_v; + } + if(changed) { + emit infoChanged(); + } + return changed; +} diff --git a/src/Camera/QGCCameraControl.h b/src/Camera/QGCCameraControl.h new file mode 100644 index 0000000..a34194e --- /dev/null +++ b/src/Camera/QGCCameraControl.h @@ -0,0 +1,422 @@ + + +#pragma once + +#include "QGCApplication.h" +#include + +class QDomNode; +class QDomNodeList; +class QGCCameraParamIO; + +Q_DECLARE_LOGGING_CATEGORY(CameraControlLog) +Q_DECLARE_LOGGING_CATEGORY(CameraControlVerboseLog) + +//----------------------------------------------------------------------------- +/// Video Stream Info +/// Encapsulates the contents of a [VIDEO_STREAM_INFORMATION](https://mavlink.io/en/messages/common.html#VIDEO_STREAM_INFORMATION) message +class QGCVideoStreamInfo : public QObject +{ + Q_OBJECT +public: + QGCVideoStreamInfo(QObject* parent, const mavlink_video_stream_information_t* si); + + Q_PROPERTY(QString uri READ uri NOTIFY infoChanged) + Q_PROPERTY(QString name READ name NOTIFY infoChanged) + Q_PROPERTY(int streamID READ streamID NOTIFY infoChanged) + Q_PROPERTY(int type READ type NOTIFY infoChanged) + Q_PROPERTY(qreal aspectRatio READ aspectRatio NOTIFY infoChanged) + Q_PROPERTY(qreal hfov READ hfov NOTIFY infoChanged) + Q_PROPERTY(bool isThermal READ isThermal NOTIFY infoChanged) + + QString uri () { return QString(_streamInfo.uri); } + QString name () { return QString(_streamInfo.name); } + qreal aspectRatio () const; + qreal hfov () const{ return _streamInfo.hfov; } + int type () const{ return _streamInfo.type; } + int streamID () const{ return _streamInfo.stream_id; } + bool isThermal () const{ return _streamInfo.flags & VIDEO_STREAM_STATUS_FLAGS_THERMAL; } + + bool update (const mavlink_video_stream_status_t* vs); + +signals: + void infoChanged (); + +private: + mavlink_video_stream_information_t _streamInfo; +}; + +//----------------------------------------------------------------------------- +/// Camera option exclusions +class QGCCameraOptionExclusion : public QObject +{ +public: + QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_); + QString param; + QString value; + QStringList exclusions; +}; + +//----------------------------------------------------------------------------- +/// Camera option ranges +class QGCCameraOptionRange : public QObject +{ +public: + QGCCameraOptionRange(QObject* parent, QString param_, QString value_, QString targetParam_, QString condition_, QStringList optNames_, QStringList optValues_); + QString param; + QString value; + QString targetParam; + QString condition; + QStringList optNames; + QStringList optValues; + QVariantList optVariants; +}; + +//----------------------------------------------------------------------------- +/// MAVLink Camera API controller +class QGCCameraControl : public FactGroup +{ + Q_OBJECT + friend class QGCCameraParamIO; +public: + QGCCameraControl(const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = nullptr); + virtual ~QGCCameraControl(); + + //-- cam_mode + enum CameraMode { + CAM_MODE_UNDEFINED = -1, + CAM_MODE_PHOTO = 0, + CAM_MODE_VIDEO = 1, + CAM_MODE_SURVEY = 2, + }; + + //-- Video Capture Status + enum VideoStatus { + VIDEO_CAPTURE_STATUS_STOPPED = 0, + VIDEO_CAPTURE_STATUS_RUNNING, + VIDEO_CAPTURE_STATUS_LAST, + VIDEO_CAPTURE_STATUS_UNDEFINED = 255 + }; + + //-- Photo Capture Status + enum PhotoStatus { + PHOTO_CAPTURE_IDLE = 0, + PHOTO_CAPTURE_IN_PROGRESS, + PHOTO_CAPTURE_INTERVAL_IDLE, + PHOTO_CAPTURE_INTERVAL_IN_PROGRESS, + PHOTO_CAPTURE_LAST, + PHOTO_CAPTURE_STATUS_UNDEFINED = 255 + }; + + //-- Photo Capture Modes + enum PhotoMode { + PHOTO_CAPTURE_SINGLE = 0, + PHOTO_CAPTURE_TIMELAPSE, + }; + + //-- Storage Status + enum StorageStatus { + STORAGE_EMPTY = STORAGE_STATUS_EMPTY, + STORAGE_UNFORMATTED = STORAGE_STATUS_UNFORMATTED, + STORAGE_READY = STORAGE_STATUS_READY, + STORAGE_NOT_SUPPORTED = STORAGE_STATUS_NOT_SUPPORTED + }; + + enum ThermalViewMode { + THERMAL_OFF = 0, + THERMAL_BLEND, + THERMAL_FULL, + THERMAL_PIP, + }; + + Q_ENUM(CameraMode) + Q_ENUM(VideoStatus) + Q_ENUM(PhotoStatus) + Q_ENUM(PhotoMode) + Q_ENUM(StorageStatus) + Q_ENUM(ThermalViewMode) + + Q_PROPERTY(int version READ version NOTIFY infoChanged) + Q_PROPERTY(QString modelName READ modelName NOTIFY infoChanged) + Q_PROPERTY(QString vendor READ vendor NOTIFY infoChanged) + Q_PROPERTY(QString firmwareVersion READ firmwareVersion NOTIFY infoChanged) + Q_PROPERTY(qreal focalLength READ focalLength NOTIFY infoChanged) + Q_PROPERTY(QSizeF sensorSize READ sensorSize NOTIFY infoChanged) + Q_PROPERTY(QSize resolution READ resolution NOTIFY infoChanged) + Q_PROPERTY(bool capturesVideo READ capturesVideo NOTIFY infoChanged) + Q_PROPERTY(bool capturesPhotos READ capturesPhotos NOTIFY infoChanged) + Q_PROPERTY(bool hasModes READ hasModes NOTIFY infoChanged) + Q_PROPERTY(bool hasZoom READ hasZoom NOTIFY infoChanged) + Q_PROPERTY(bool hasFocus READ hasFocus NOTIFY infoChanged) + Q_PROPERTY(bool hasVideoStream READ hasVideoStream NOTIFY infoChanged) + Q_PROPERTY(bool photosInVideoMode READ photosInVideoMode NOTIFY infoChanged) + Q_PROPERTY(bool videoInPhotoMode READ videoInPhotoMode NOTIFY infoChanged) + Q_PROPERTY(bool isBasic READ isBasic NOTIFY infoChanged) + Q_PROPERTY(quint32 storageFree READ storageFree NOTIFY storageFreeChanged) + Q_PROPERTY(QString storageFreeStr READ storageFreeStr NOTIFY storageFreeChanged) + Q_PROPERTY(quint32 storageTotal READ storageTotal NOTIFY storageTotalChanged) + Q_PROPERTY(int batteryRemaining READ batteryRemaining NOTIFY batteryRemainingChanged) + Q_PROPERTY(QString batteryRemainingStr READ batteryRemainingStr NOTIFY batteryRemainingChanged) + Q_PROPERTY(bool paramComplete READ paramComplete NOTIFY parametersReady) + + Q_PROPERTY(qreal zoomLevel READ zoomLevel WRITE setZoomLevel NOTIFY zoomLevelChanged) + Q_PROPERTY(qreal focusLevel READ focusLevel WRITE setFocusLevel NOTIFY focusLevelChanged) + + Q_PROPERTY(Fact* exposureMode READ exposureMode NOTIFY parametersReady) + Q_PROPERTY(Fact* ev READ ev NOTIFY parametersReady) + Q_PROPERTY(Fact* iso READ iso NOTIFY parametersReady) + Q_PROPERTY(Fact* shutterSpeed READ shutterSpeed NOTIFY parametersReady) + Q_PROPERTY(Fact* aperture READ aperture NOTIFY parametersReady) + Q_PROPERTY(Fact* wb READ wb NOTIFY parametersReady) + Q_PROPERTY(Fact* mode READ mode NOTIFY parametersReady) + + Q_PROPERTY(QStringList activeSettings READ activeSettings NOTIFY activeSettingsChanged) + Q_PROPERTY(VideoStatus videoStatus READ videoStatus NOTIFY videoStatusChanged) + Q_PROPERTY(PhotoStatus photoStatus READ photoStatus NOTIFY photoStatusChanged) + Q_PROPERTY(CameraMode cameraMode READ cameraMode WRITE setCameraMode NOTIFY cameraModeChanged) + Q_PROPERTY(StorageStatus storageStatus READ storageStatus NOTIFY storageStatusChanged) + Q_PROPERTY(qreal photoLapse READ photoLapse WRITE setPhotoLapse NOTIFY photoLapseChanged) + Q_PROPERTY(int photoLapseCount READ photoLapseCount WRITE setPhotoLapseCount NOTIFY photoLapseCountChanged) + Q_PROPERTY(PhotoMode photoMode READ photoMode WRITE setPhotoMode NOTIFY photoModeChanged) + Q_PROPERTY(int currentStream READ currentStream WRITE setCurrentStream NOTIFY currentStreamChanged) + Q_PROPERTY(bool autoStream READ autoStream NOTIFY autoStreamChanged) + Q_PROPERTY(QmlObjectListModel* streams READ streams NOTIFY streamsChanged) + Q_PROPERTY(QGCVideoStreamInfo* currentStreamInstance READ currentStreamInstance NOTIFY currentStreamChanged) + Q_PROPERTY(QGCVideoStreamInfo* thermalStreamInstance READ thermalStreamInstance NOTIFY thermalStreamChanged) + Q_PROPERTY(quint32 recordTime READ recordTime NOTIFY recordTimeChanged) + Q_PROPERTY(QString recordTimeStr READ recordTimeStr NOTIFY recordTimeChanged) + Q_PROPERTY(QStringList streamLabels READ streamLabels NOTIFY streamLabelsChanged) + Q_PROPERTY(ThermalViewMode thermalMode READ thermalMode WRITE setThermalMode NOTIFY thermalModeChanged) + Q_PROPERTY(double thermalOpacity READ thermalOpacity WRITE setThermalOpacity NOTIFY thermalOpacityChanged) + + Q_INVOKABLE virtual void setVideoMode (); + Q_INVOKABLE virtual void setPhotoMode (); + Q_INVOKABLE virtual void toggleMode (); + Q_INVOKABLE virtual bool takePhoto (); + Q_INVOKABLE virtual bool stopTakePhoto (); + Q_INVOKABLE virtual bool startVideo (); + Q_INVOKABLE virtual bool stopVideo (); + Q_INVOKABLE virtual bool toggleVideo (); + Q_INVOKABLE virtual void resetSettings (); + Q_INVOKABLE virtual void formatCard (int id = 1); + Q_INVOKABLE virtual void stepZoom (int direction); + Q_INVOKABLE virtual void startZoom (int direction); + Q_INVOKABLE virtual void stopZoom (); + Q_INVOKABLE virtual void stopStream (); + Q_INVOKABLE virtual void resumeStream (); + + virtual int version () { return _version; } + virtual QString modelName () { return _modelName; } + virtual QString vendor () { return _vendor; } + virtual QString firmwareVersion (); + virtual qreal focalLength () { return static_cast(_info.focal_length); } + virtual QSizeF sensorSize () { return QSizeF(static_cast(_info.sensor_size_h), static_cast(_info.sensor_size_v)); } + virtual QSize resolution () { return QSize(_info.resolution_h, _info.resolution_v); } + virtual bool capturesVideo () { return _info.flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO; } + virtual bool capturesPhotos () { return _info.flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE; } + virtual bool hasModes () { return _info.flags & CAMERA_CAP_FLAGS_HAS_MODES; } + virtual bool hasZoom () { return _info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM; } + virtual bool hasFocus () { return _info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS; } + virtual bool hasVideoStream () { return _info.flags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM; } + virtual bool photosInVideoMode () { return _info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE; } + virtual bool videoInPhotoMode () { return _info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE; } + + virtual int compID () { return _compID; } + virtual bool isBasic () { return _settings.size() == 0; } + virtual VideoStatus videoStatus (); + virtual PhotoStatus photoStatus (); + virtual PhotoMode photoMode () { return _photoMode; } + virtual qreal photoLapse () { return _photoLapse; } + virtual int photoLapseCount () { return _photoLapseCount; } + virtual CameraMode cameraMode () { return _cameraMode; } + virtual StorageStatus storageStatus () { return _storageStatus; } + virtual QStringList activeSettings (); + virtual quint32 storageFree () { return _storageFree; } + virtual QString storageFreeStr (); + virtual quint32 storageTotal () { return _storageTotal; } + virtual int batteryRemaining () { return _batteryRemaining; } + virtual QString batteryRemainingStr (); + virtual bool paramComplete () { return _paramComplete; } + virtual qreal zoomLevel () { return _zoomLevel; } + virtual qreal focusLevel () { return _focusLevel; } + + virtual QmlObjectListModel* streams () { return &_streams; } + virtual QGCVideoStreamInfo* currentStreamInstance(); + virtual QGCVideoStreamInfo* thermalStreamInstance(); + virtual int currentStream () { return _currentStream; } + virtual void setCurrentStream (int stream); + virtual bool autoStream (); + virtual quint32 recordTime () { return _recordTime; } + virtual QString recordTimeStr (); + + virtual Fact* exposureMode (); + virtual Fact* ev (); + virtual Fact* iso (); + virtual Fact* shutterSpeed (); + virtual Fact* aperture (); + virtual Fact* wb (); + virtual Fact* mode (); + + /// Stream names to show the user (for selection) + virtual QStringList streamLabels () { return _streamLabels; } + + virtual ThermalViewMode thermalMode () { return _thermalMode; } + virtual void setThermalMode (ThermalViewMode mode); + virtual double thermalOpacity () { return _thermalOpacity; } + virtual void setThermalOpacity (double val); + + virtual void setZoomLevel (qreal level); + virtual void setFocusLevel (qreal level); + virtual void setCameraMode (CameraMode mode); + virtual void setPhotoMode (PhotoMode mode); + virtual void setPhotoLapse (qreal interval); + virtual void setPhotoLapseCount (int count); + + virtual void handleSettings (const mavlink_camera_settings_t& settings); + virtual void handleCaptureStatus (const mavlink_camera_capture_status_t& capStatus); + virtual void handleParamAck (const mavlink_param_ext_ack_t& ack); + virtual void handleParamValue (const mavlink_param_ext_value_t& value); + virtual void handleStorageInfo (const mavlink_storage_information_t& st); + virtual void handleBatteryStatus (const mavlink_battery_status_t& bs); + virtual void handleVideoInfo (const mavlink_video_stream_information_t *vi); + virtual void handleVideoStatus (const mavlink_video_stream_status_t *vs); + + /// Notify controller a parameter has changed + virtual void factChanged (Fact* pFact); + /// Allow controller to modify or invalidate incoming parameter + virtual bool incomingParameter (Fact* pFact, QVariant& newValue); + /// Allow controller to modify or invalidate parameter change + virtual bool validateParameter (Fact* pFact, QVariant& newValue); + + // Known Parameters + static const char* kCAM_EV; + static const char* kCAM_EXPMODE; + static const char* kCAM_ISO; + static const char* kCAM_SHUTTERSPD; + static const char* kCAM_APERTURE; + static const char* kCAM_WBMODE; + static const char* kCAM_MODE; + +signals: + void infoChanged (); + void videoStatusChanged (); + void photoStatusChanged (); + void photoModeChanged (); + void photoLapseChanged (); + void photoLapseCountChanged (); + void cameraModeChanged (); + void activeSettingsChanged (); + void storageFreeChanged (); + void storageTotalChanged (); + void batteryRemainingChanged (); + void dataReady (QByteArray data); + void parametersReady (); + void zoomLevelChanged (); + void focusLevelChanged (); + void streamsChanged (); + void currentStreamChanged (); + void thermalStreamChanged (); + void autoStreamChanged (); + void recordTimeChanged (); + void streamLabelsChanged (); + void thermalModeChanged (); + void thermalOpacityChanged (); + void storageStatusChanged (); + +protected: + virtual void _setVideoStatus (VideoStatus status); + virtual void _setPhotoStatus (PhotoStatus status); + virtual void _setCameraMode (CameraMode mode); + virtual void _requestStreamInfo (uint8_t streamID); + virtual void _requestStreamStatus (uint8_t streamID); + virtual QGCVideoStreamInfo* _findStream (uint8_t streamID, bool report = true); + virtual QGCVideoStreamInfo* _findStream (const QString name); + +protected slots: + virtual void _initWhenReady (); + virtual void _requestCameraSettings (); + virtual void _requestAllParameters (); + virtual void _requestParamUpdates (); + virtual void _requestCaptureStatus (); + virtual void _requestStorageInfo (); + virtual void _downloadFinished (); + virtual void _mavCommandResult (int vehicleId, int component, int command, int result, bool noReponseFromVehicle); + virtual void _dataReady (QByteArray data); + virtual void _paramDone (); + virtual void _streamTimeout (); + virtual void _streamStatusTimeout (); + virtual void _recTimerHandler (); + virtual void _checkForVideoStreams (); + +private: + bool _handleLocalization (QByteArray& bytes); + bool _replaceLocaleStrings (const QDomNode node, QByteArray& bytes); + bool _loadCameraDefinitionFile (QByteArray& bytes); + bool _loadConstants (const QDomNodeList nodeList); + bool _loadSettings (const QDomNodeList nodeList); + void _processRanges (); + bool _processCondition (const QString condition); + bool _processConditionTest (const QString conditionTest); + bool _loadNameValue (QDomNode option, const QString factName, FactMetaData* metaData, QString& optName, QString& optValue, QVariant& optVariant); + bool _loadRanges (QDomNode option, const QString factName, QString paramValue); + void _updateActiveList (); + void _updateRanges (Fact* pFact); + void _httpRequest (const QString& url); + void _handleDefinitionFile (const QString& url); + void _ftpDownloadComplete (const QString& fileName, const QString& errorMsg); + + QStringList _loadExclusions (QDomNode option); + QStringList _loadUpdates (QDomNode option); + QString _getParamName (const char* param_id); + +protected: + Vehicle* _vehicle = nullptr; + int _compID = 0; + mavlink_camera_information_t _info; + int _version = 0; + bool _cached = false; + bool _paramComplete = false; + qreal _zoomLevel = 0.0; + qreal _focusLevel = 0.0; + uint32_t _storageFree = 0; + uint32_t _storageTotal = 0; + int _batteryRemaining = -1; + QNetworkAccessManager* _netManager = nullptr; + QString _modelName; + QString _vendor; + QString _cacheFile; + CameraMode _cameraMode = CAM_MODE_UNDEFINED; + StorageStatus _storageStatus = STORAGE_NOT_SUPPORTED; + PhotoMode _photoMode = PHOTO_CAPTURE_SINGLE; + qreal _photoLapse = 1.0; + int _photoLapseCount = 0; + VideoStatus _video_status = VIDEO_CAPTURE_STATUS_UNDEFINED; + PhotoStatus _photo_status = PHOTO_CAPTURE_STATUS_UNDEFINED; + QStringList _activeSettings; + QStringList _settings; + QTimer _captureStatusTimer; + QList _valueExclusions; + QList _optionRanges; + QMap _originalOptNames; + QMap _originalOptValues; + QMap _paramIO; + int _storageInfoRetries = 0; + int _captureInfoRetries = 0; + bool _resetting = false; + QTimer _recTimer; + QTime _recTime; + uint32_t _recordTime = 0; + //-- Parameters that require a full update + QMap _requestUpdates; + QStringList _updatesToRequest; + //-- Video Streams + int _requestCount = 0; + int _currentStream = 0; + int _expectedCount = 1; + QTimer _streamInfoTimer; + QTimer _streamStatusTimer; + QmlObjectListModel _streams; + QStringList _streamLabels; + ThermalViewMode _thermalMode = THERMAL_BLEND; + double _thermalOpacity = 85.0; +}; diff --git a/src/Camera/QGCCameraIO.cc b/src/Camera/QGCCameraIO.cc new file mode 100644 index 0000000..13d3837 --- /dev/null +++ b/src/Camera/QGCCameraIO.cc @@ -0,0 +1,372 @@ + + +#include "QGCCameraControl.h" +#include "QGCCameraIO.h" + +QGC_LOGGING_CATEGORY(CameraIOLog, "CameraIOLog") +QGC_LOGGING_CATEGORY(CameraIOLogVerbose, "CameraIOLogVerbose") + +//----------------------------------------------------------------------------- +QGCCameraParamIO::QGCCameraParamIO(QGCCameraControl *control, Fact* fact, Vehicle *vehicle) + : QObject(control) + , _control(control) + , _fact(fact) + , _vehicle(vehicle) + , _sentRetries(0) + , _requestRetries(0) + , _done(false) + , _updateOnSet(false) + , _forceUIUpdate(false) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + _paramWriteTimer.setSingleShot(true); + _paramWriteTimer.setInterval(3000); + _paramRequestTimer.setSingleShot(true); + _paramRequestTimer.setInterval(3500); + if(_fact->writeOnly()) { + //-- Write mode is always "done" as it won't ever read + _done = true; + } else { + connect(&_paramRequestTimer, &QTimer::timeout, this, &QGCCameraParamIO::_paramRequestTimeout); + } + connect(&_paramWriteTimer, &QTimer::timeout, this, &QGCCameraParamIO::_paramWriteTimeout); + connect(_fact, &Fact::rawValueChanged, this, &QGCCameraParamIO::_factChanged); + connect(_fact, &Fact::_containerRawValueChanged, this, &QGCCameraParamIO::_containerRawValueChanged); + _pMavlink = qgcApp()->toolbox()->mavlinkProtocol(); + //-- TODO: Even though we don't use anything larger than 32-bit, this should + // probably be updated. + switch (_fact->type()) { + case FactMetaData::valueTypeUint8: + case FactMetaData::valueTypeBool: + _mavParamType = MAV_PARAM_EXT_TYPE_UINT8; + break; + case FactMetaData::valueTypeInt8: + _mavParamType = MAV_PARAM_EXT_TYPE_INT8; + break; + case FactMetaData::valueTypeUint16: + _mavParamType = MAV_PARAM_EXT_TYPE_UINT16; + break; + case FactMetaData::valueTypeInt16: + _mavParamType = MAV_PARAM_EXT_TYPE_INT16; + break; + case FactMetaData::valueTypeUint32: + _mavParamType = MAV_PARAM_EXT_TYPE_UINT32; + break; + case FactMetaData::valueTypeUint64: + _mavParamType = MAV_PARAM_EXT_TYPE_UINT64; + break; + case FactMetaData::valueTypeInt64: + _mavParamType = MAV_PARAM_EXT_TYPE_INT64; + break; + case FactMetaData::valueTypeFloat: + _mavParamType = MAV_PARAM_EXT_TYPE_REAL32; + break; + case FactMetaData::valueTypeDouble: + _mavParamType = MAV_PARAM_EXT_TYPE_REAL64; + break; + //-- String and custom are the same for now + case FactMetaData::valueTypeString: + case FactMetaData::valueTypeCustom: + _mavParamType = MAV_PARAM_EXT_TYPE_CUSTOM; + break; + default: + qWarning() << "Unsupported fact type" << _fact->type() << "for" << _fact->name(); + case FactMetaData::valueTypeInt32: + _mavParamType = MAV_PARAM_EXT_TYPE_INT32; + break; + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::setParamRequest() +{ + if(!_fact->writeOnly()) { + _paramRequestReceived = false; + _requestRetries = 0; + _paramRequestTimer.start(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_factChanged(QVariant value) +{ + if(!_forceUIUpdate) { + Q_UNUSED(value); + qCDebug(CameraIOLog) << "UI Fact" << _fact->name() << "changed to" << value; + _control->factChanged(_fact); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_containerRawValueChanged(const QVariant value) +{ + if(!_fact->readOnly()) { + Q_UNUSED(value); + qCDebug(CameraIOLog) << "Update Fact from camera" << _fact->name(); + _sentRetries = 0; + _sendParameter(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::sendParameter(bool updateUI) +{ + qCDebug(CameraIOLog) << "Send Fact" << _fact->name(); + _sentRetries = 0; + _updateOnSet = updateUI; + _sendParameter(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_sendParameter() +{ + WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); + if (!weakLink.expired()) { + SharedLinkInterfacePtr sharedLink = weakLink.lock(); + + mavlink_param_ext_set_t p; + memset(&p, 0, sizeof(mavlink_param_ext_set_t)); + param_ext_union_t union_value; + mavlink_message_t msg; + FactMetaData::ValueType_t factType = _fact->type(); + p.param_type = _mavParamType; + switch (factType) { + case FactMetaData::valueTypeUint8: + case FactMetaData::valueTypeBool: + union_value.param_uint8 = static_cast(_fact->rawValue().toUInt()); + break; + case FactMetaData::valueTypeInt8: + union_value.param_int8 = static_cast(_fact->rawValue().toInt()); + break; + case FactMetaData::valueTypeUint16: + union_value.param_uint16 = static_cast(_fact->rawValue().toUInt()); + break; + case FactMetaData::valueTypeInt16: + union_value.param_int16 = static_cast(_fact->rawValue().toInt()); + break; + case FactMetaData::valueTypeUint32: + union_value.param_uint32 = static_cast(_fact->rawValue().toUInt()); + break; + case FactMetaData::valueTypeInt64: + union_value.param_int64 = static_cast(_fact->rawValue().toLongLong()); + break; + case FactMetaData::valueTypeUint64: + union_value.param_uint64 = static_cast(_fact->rawValue().toULongLong()); + break; + case FactMetaData::valueTypeFloat: + union_value.param_float = _fact->rawValue().toFloat(); + break; + case FactMetaData::valueTypeDouble: + union_value.param_double = _fact->rawValue().toDouble(); + break; + //-- String and custom are the same for now + case FactMetaData::valueTypeString: + case FactMetaData::valueTypeCustom: + { + QByteArray custom = _fact->rawValue().toByteArray(); + memcpy(union_value.bytes, custom.data(), static_cast(std::max(custom.size(), static_cast(MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN)))); + } + break; + default: + qCritical() << "Unsupported fact type" << factType << "for" << _fact->name(); + case FactMetaData::valueTypeInt32: + union_value.param_int32 = static_cast(_fact->rawValue().toInt()); + break; + } + memcpy(&p.param_value[0], &union_value.bytes[0], MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); + p.target_system = static_cast(_vehicle->id()); + p.target_component = static_cast(_control->compID()); + strncpy(p.param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN); + mavlink_msg_param_ext_set_encode_chan( + static_cast(_pMavlink->getSystemId()), + static_cast(_pMavlink->getComponentId()), + sharedLink->mavlinkChannel(), + &msg, + &p); + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } + _paramWriteTimer.start(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_paramWriteTimeout() +{ + if(++_sentRetries > 3) { + qCWarning(CameraIOLog) << "No response for param set:" << _fact->name(); + _updateOnSet = false; + } else { + //-- Send it again + qCDebug(CameraIOLog) << "Param set retry:" << _fact->name() << _sentRetries; + _sendParameter(); + _paramWriteTimer.start(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::handleParamAck(const mavlink_param_ext_ack_t& ack) +{ + _paramWriteTimer.stop(); + if(ack.param_result == PARAM_ACK_ACCEPTED) { + QVariant val = _valueFromMessage(ack.param_value, ack.param_type); + if(_fact->rawValue() != val) { + _fact->_containerSetRawValue(val); + if(_updateOnSet) { + _updateOnSet = false; + _control->factChanged(_fact); + } + } + } else if(ack.param_result == PARAM_ACK_IN_PROGRESS) { + //-- Wait a bit longer for this one + qCDebug(CameraIOLogVerbose) << "Param set in progress:" << _fact->name(); + _paramWriteTimer.start(); + } else { + if(ack.param_result == PARAM_ACK_FAILED) { + if(++_sentRetries < 3) { + //-- Try again + qCWarning(CameraIOLog) << "Param set failed:" << _fact->name() << _sentRetries; + _paramWriteTimer.start(); + } + return; + } else if(ack.param_result == PARAM_ACK_VALUE_UNSUPPORTED) { + qCWarning(CameraIOLog) << "Param set unsuported:" << _fact->name(); + } + //-- If UI changed and value was not set, restore UI + QVariant val = _valueFromMessage(ack.param_value, ack.param_type); + if(_fact->rawValue() != val) { + if(_control->validateParameter(_fact, val)) { + _fact->_containerSetRawValue(val); + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::handleParamValue(const mavlink_param_ext_value_t& value) +{ + _paramRequestTimer.stop(); + QVariant newValue = _valueFromMessage(value.param_value, value.param_type); + if(_control->incomingParameter(_fact, newValue)) { + _fact->_containerSetRawValue(newValue); + } + _paramRequestReceived = true; + if(_forceUIUpdate) { + emit _fact->rawValueChanged(_fact->rawValue()); + emit _fact->valueChanged(_fact->rawValue()); + _forceUIUpdate = false; + } + if(!_done) { + _done = true; + _control->_paramDone(); + } + qCDebug(CameraIOLog) << QString("handleParamValue() %1 %2").arg(_fact->name()).arg(_fact->rawValueString()); +} + +//----------------------------------------------------------------------------- +QVariant +QGCCameraParamIO::_valueFromMessage(const char* value, uint8_t param_type) +{ + QVariant var; + param_ext_union_t u; + memcpy(u.bytes, value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); + switch (param_type) { + case MAV_PARAM_EXT_TYPE_REAL32: + var = QVariant(u.param_float); + break; + case MAV_PARAM_EXT_TYPE_UINT8: + var = QVariant(u.param_uint8); + break; + case MAV_PARAM_EXT_TYPE_INT8: + var = QVariant(u.param_int8); + break; + case MAV_PARAM_EXT_TYPE_UINT16: + var = QVariant(u.param_uint16); + break; + case MAV_PARAM_EXT_TYPE_INT16: + var = QVariant(u.param_int16); + break; + case MAV_PARAM_EXT_TYPE_UINT32: + var = QVariant(u.param_uint32); + break; + case MAV_PARAM_EXT_TYPE_INT32: + var = QVariant(u.param_int32); + break; + case MAV_PARAM_EXT_TYPE_UINT64: + var = QVariant(static_cast(u.param_uint64)); + break; + case MAV_PARAM_EXT_TYPE_INT64: + var = QVariant(static_cast(u.param_int64)); + break; + case MAV_PARAM_EXT_TYPE_CUSTOM: + var = QVariant(QByteArray(value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN)); + break; + default: + var = QVariant(0); + qCritical() << "Invalid param_type used for camera setting:" << param_type; + } + return var; +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_paramRequestTimeout() +{ + if(++_requestRetries > 3) { + qCWarning(CameraIOLog) << "No response for param request:" << _fact->name(); + if(!_done) { + _done = true; + _control->_paramDone(); + } + } else { + //-- Request it again + qCDebug(CameraIOLog) << "Param request retry:" << _fact->name(); + paramRequest(false); + _paramRequestTimer.start(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::paramRequest(bool reset) +{ + //-- If it's write only, we don't request it. + if(_fact->writeOnly()) { + if(!_done) { + _done = true; + _control->_paramDone(); + } + return; + } + if(reset) { + _requestRetries = 0; + _forceUIUpdate = true; + } + qCDebug(CameraIOLog) << "Request parameter:" << _fact->name(); + WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); + if (!weakLink.expired()) { + SharedLinkInterfacePtr sharedLink = weakLink.lock(); + + char param_id[MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]; + memset(param_id, 0, sizeof(param_id)); + strncpy(param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN); + mavlink_message_t msg; + mavlink_msg_param_ext_request_read_pack_chan( + static_cast(_pMavlink->getSystemId()), + static_cast(_pMavlink->getComponentId()), + sharedLink->mavlinkChannel(), + &msg, + static_cast(_vehicle->id()), + static_cast(_control->compID()), + param_id, + -1); + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } + _paramRequestTimer.start(); +} diff --git a/src/Camera/QGCCameraIO.h b/src/Camera/QGCCameraIO.h new file mode 100644 index 0000000..8db7d15 --- /dev/null +++ b/src/Camera/QGCCameraIO.h @@ -0,0 +1,72 @@ + +#pragma once + +#include "QGCApplication.h" +#include + +class QGCCameraControl; + +Q_DECLARE_LOGGING_CATEGORY(CameraIOLog) +Q_DECLARE_LOGGING_CATEGORY(CameraIOLogVerbose) + +MAVPACKED( +typedef struct { + union { + float param_float; + double param_double; + int64_t param_int64; + uint64_t param_uint64; + int32_t param_int32; + uint32_t param_uint32; + int16_t param_int16; + uint16_t param_uint16; + int8_t param_int8; + uint8_t param_uint8; + uint8_t bytes[MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN]; + }; + uint8_t type; +}) param_ext_union_t; + +//----------------------------------------------------------------------------- +/// Camera parameter handler. +class QGCCameraParamIO : public QObject +{ +public: + QGCCameraParamIO(QGCCameraControl* control, Fact* fact, Vehicle* vehicle); + + void handleParamAck (const mavlink_param_ext_ack_t& ack); + void handleParamValue (const mavlink_param_ext_value_t& value); + void setParamRequest (); + bool paramDone () const { return _done; } + void paramRequest (bool reset = true); + void sendParameter (bool updateUI = false); + + QStringList optNames; + QVariantList optVariants; + +private slots: + void _paramWriteTimeout (); + void _paramRequestTimeout (); + void _factChanged (QVariant value); + void _containerRawValueChanged (const QVariant value); + +private: + void _sendParameter (); + QVariant _valueFromMessage (const char* value, uint8_t param_type); + +private: + QGCCameraControl* _control; + Fact* _fact; + Vehicle* _vehicle; + int _sentRetries; + int _requestRetries; + bool _paramRequestReceived; + QTimer _paramWriteTimer; + QTimer _paramRequestTimer; + bool _done; + bool _updateOnSet; + MAV_PARAM_EXT_TYPE _mavParamType; + MAVLinkProtocol* _pMavlink; + bool _forceUIUpdate; +}; + diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc new file mode 100644 index 0000000..ed53c21 --- /dev/null +++ b/src/Camera/QGCCameraManager.cc @@ -0,0 +1,518 @@ + + +#include "QGCApplication.h" +#include "QGCCameraManager.h" +#include "JoystickManager.h" + +QGC_LOGGING_CATEGORY(CameraManagerLog, "CameraManagerLog") + +//----------------------------------------------------------------------------- +QGCCameraManager::CameraStruct::CameraStruct(QObject* parent, uint8_t compID_) + : QObject(parent) + , compID(compID_) +{ +} + +//----------------------------------------------------------------------------- +QGCCameraManager::QGCCameraManager(Vehicle *vehicle) + : _vehicle(vehicle) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + qCDebug(CameraManagerLog) << "QGCCameraManager Created"; + connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &QGCCameraManager::_vehicleReady); + connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &QGCCameraManager::_mavlinkMessageReceived); + connect(&_cameraTimer, &QTimer::timeout, this, &QGCCameraManager::_cameraTimeout); + _cameraTimer.setSingleShot(false); + _lastZoomChange.start(); + _lastCameraChange.start(); + _cameraTimer.start(500); +} + +//----------------------------------------------------------------------------- +QGCCameraManager::~QGCCameraManager() +{ +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::setCurrentCamera(int sel) +{ + if(sel != _currentCamera && sel >= 0 && sel < _cameras.count()) { + _currentCamera = sel; + emit currentCameraChanged(); + emit streamChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_vehicleReady(bool ready) +{ + qCDebug(CameraManagerLog) << "_vehicleReady(" << ready << ")"; + if(ready) { + if(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle() == _vehicle) { + _vehicleReadyState = true; + JoystickManager *pJoyMgr = qgcApp()->toolbox()->joystickManager(); + _activeJoystickChanged(pJoyMgr->activeJoystick()); + connect(pJoyMgr, &JoystickManager::activeJoystickChanged, this, &QGCCameraManager::_activeJoystickChanged); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t& message) +{ + //-- Only pay attention to camera components, as identified by their compId + if(message.sysid == _vehicle->id() && (message.compid >= MAV_COMP_ID_CAMERA && message.compid <= MAV_COMP_ID_CAMERA6)) { + switch (message.msgid) { + case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS: + _handleCaptureStatus(message); + break; + case MAVLINK_MSG_ID_STORAGE_INFORMATION: + _handleStorageInfo(message); + break; + 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; + case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION: + _handleVideoStreamInfo(message); + break; + case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS: + _handleVideoStreamStatus(message); + break; + case MAVLINK_MSG_ID_BATTERY_STATUS: + _handleBatteryStatus(message); + break; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleHeartbeat(const mavlink_message_t &message) +{ + //-- First time hearing from this one? + QString sCompID = QString::number(message.compid); + if(!_cameraInfoRequest.contains(sCompID)) { + qCDebug(CameraManagerLog) << "Hearbeat from " << message.compid; + CameraStruct* pInfo = new CameraStruct(this, message.compid); + pInfo->lastHeartbeat.start(); + _cameraInfoRequest[sCompID] = pInfo; + //-- Request camera info + _requestCameraInfo(message.compid); + } else { + if(_cameraInfoRequest[sCompID]) { + CameraStruct* pInfo = _cameraInfoRequest[sCompID]; + //-- Check if we have indeed received the camera info + if(pInfo->infoReceived) { + //-- We have it. Just update the heartbeat timeout + pInfo->lastHeartbeat.start(); + } else { + //-- Try again. Maybe. + if(pInfo->lastHeartbeat.elapsed() > 2000) { + if(pInfo->tryCount > 10) { + if(!pInfo->gaveUp) { + pInfo->gaveUp = true; + qWarning() << "Giving up requesting camera info from" << _vehicle->id() << message.compid; + } + } else { + pInfo->tryCount++; + //-- Request camera info again. + _requestCameraInfo(message.compid); + } + } + } + } else { + qWarning() << "_cameraInfoRequest[" << sCompID << "] is null"; + } + } +} + +//----------------------------------------------------------------------------- +QGCCameraControl* +QGCCameraManager::currentCameraInstance() +{ + if(_currentCamera < _cameras.count() && _cameras.count()) { + QGCCameraControl* pCamera = qobject_cast(_cameras[_currentCamera]); + return pCamera; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraManager::currentStreamInstance() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + QGCVideoStreamInfo* pInfo = pCamera->currentStreamInstance(); + return pInfo; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraManager::thermalStreamInstance() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + QGCVideoStreamInfo* pInfo = pCamera->thermalStreamInstance(); + return pInfo; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCCameraControl* +QGCCameraManager::_findCamera(int id) +{ + for(int i = 0; i < _cameras.count(); i++) { + if(_cameras[i]) { + QGCCameraControl* pCamera = qobject_cast(_cameras[i]); + if(pCamera) { + if(pCamera->compID() == id) { + return pCamera; + } + } else { + qCritical() << "Null QGCCameraControl instance"; + } + } + } + //qWarning() << "Camera component id not found:" << id; + return nullptr; +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message) +{ + //-- Have we requested it? + QString sCompID = QString::number(message.compid); + if(_cameraInfoRequest.contains(sCompID) && !_cameraInfoRequest[sCompID]->infoReceived) { + //-- Flag it as done + _cameraInfoRequest[sCompID]->infoReceived = true; + mavlink_camera_information_t info; + mavlink_msg_camera_information_decode(&message, &info); + qCDebug(CameraManagerLog) << "_handleCameraInfo:" << reinterpret_cast(info.model_name) << reinterpret_cast(info.vendor_name) << "Comp ID:" << message.compid; + QGCCameraControl* pCamera = _vehicle->firmwarePlugin()->createCameraControl(&info, _vehicle, message.compid, this); + if(pCamera) { + QQmlEngine::setObjectOwnership(pCamera, QQmlEngine::CppOwnership); + _cameras.append(pCamera); + _cameraLabels << pCamera->modelName(); + emit camerasChanged(); + emit cameraLabelsChanged(); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_cameraTimeout() +{ + //-- Iterate cameras + foreach(QString sCompID, _cameraInfoRequest.keys()) { + if(_cameraInfoRequest[sCompID]) { + CameraStruct* pInfo = _cameraInfoRequest[sCompID]; + //-- Have we received a camera info message? + if(pInfo->infoReceived) { + //-- Has the camera stopped talking to us? + if(pInfo->lastHeartbeat.elapsed() > 5000) { + //-- Camera is gone. Remove it. + bool autoStream = false; + QGCCameraControl* pCamera = _findCamera(pInfo->compID); + if(pCamera) { + qWarning() << "Camera" << pCamera->modelName() << "stopped transmitting. Removing from list."; + int idx = _cameraLabels.indexOf(pCamera->modelName()); + if(idx >= 0) { + _cameraLabels.removeAt(idx); + } + idx = _cameras.indexOf(pCamera); + if(idx >= 0) { + _cameras.removeAt(idx); + } + autoStream = pCamera->autoStream(); + pCamera->deleteLater(); + delete pInfo; + } + _cameraInfoRequest.remove(sCompID); + emit cameraLabelsChanged(); + //-- If we have another camera, switch current camera. + if(_cameras.count()) { + setCurrentCamera(0); + } else { + //-- We're out of cameras + emit camerasChanged(); + if(autoStream) { + emit streamChanged(); + } + } + //-- Exit loop. + return; + } + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleCaptureStatus(const mavlink_message_t &message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_camera_capture_status_t cap; + mavlink_msg_camera_capture_status_decode(&message, &cap); + pCamera->handleCaptureStatus(cap); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleStorageInfo(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_storage_information_t st; + mavlink_msg_storage_information_decode(&message, &st); + pCamera->handleStorageInfo(st); + } +} + +//----------------------------------------------------------------------------- +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::_handleVideoStreamInfo(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_video_stream_information_t streamInfo; + mavlink_msg_video_stream_information_decode(&message, &streamInfo); + pCamera->handleVideoInfo(&streamInfo); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleVideoStreamStatus(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_video_stream_status_t streamStatus; + mavlink_msg_video_stream_status_decode(&message, &streamStatus); + pCamera->handleVideoStatus(&streamStatus); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleBatteryStatus(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_battery_status_t batteryStatus; + mavlink_msg_battery_status_decode(&message, &batteryStatus); + pCamera->handleBatteryStatus(batteryStatus); + } +} + +//----------------------------------------------------------------------------- +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 + 1); // Do Request + } +} + +//---------------------------------------------------------------------------------------- +void +QGCCameraManager::_activeJoystickChanged(Joystick* joystick) +{ + qCDebug(CameraManagerLog) << "Joystick changed"; + if(_activeJoystick) { + disconnect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom); + disconnect(_activeJoystick, &Joystick::startContinuousZoom, this, &QGCCameraManager::_startZoom); + disconnect(_activeJoystick, &Joystick::stopContinuousZoom, this, &QGCCameraManager::_stopZoom); + disconnect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera); + disconnect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream); + disconnect(_activeJoystick, &Joystick::triggerCamera, this, &QGCCameraManager::_triggerCamera); + disconnect(_activeJoystick, &Joystick::startVideoRecord, this, &QGCCameraManager::_startVideoRecording); + disconnect(_activeJoystick, &Joystick::stopVideoRecord, this, &QGCCameraManager::_stopVideoRecording); + disconnect(_activeJoystick, &Joystick::toggleVideoRecord, this, &QGCCameraManager::_toggleVideoRecording); + } + _activeJoystick = joystick; + if(_activeJoystick) { + connect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom); + connect(_activeJoystick, &Joystick::startContinuousZoom, this, &QGCCameraManager::_startZoom); + connect(_activeJoystick, &Joystick::stopContinuousZoom, this, &QGCCameraManager::_stopZoom); + connect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera); + connect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream); + connect(_activeJoystick, &Joystick::triggerCamera, this, &QGCCameraManager::_triggerCamera); + connect(_activeJoystick, &Joystick::startVideoRecord, this, &QGCCameraManager::_startVideoRecording); + connect(_activeJoystick, &Joystick::stopVideoRecord, this, &QGCCameraManager::_stopVideoRecording); + connect(_activeJoystick, &Joystick::toggleVideoRecord, this, &QGCCameraManager::_toggleVideoRecording); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_triggerCamera() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->takePhoto(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_startVideoRecording() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->startVideo(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stopVideoRecording() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->stopVideo(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_toggleVideoRecording() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->toggleVideo(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stepZoom(int direction) +{ + if(_lastZoomChange.elapsed() > 40) { + _lastZoomChange.start(); + qCDebug(CameraManagerLog) << "Step Camera Zoom" << direction; + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->stepZoom(direction); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_startZoom(int direction) +{ + qCDebug(CameraManagerLog) << "Start Camera Zoom" << direction; + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->startZoom(direction); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stopZoom() +{ + qCDebug(CameraManagerLog) << "Stop Camera Zoom"; + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->stopZoom(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stepCamera(int direction) +{ + if(_lastCameraChange.elapsed() > 1000) { + _lastCameraChange.start(); + qCDebug(CameraManagerLog) << "Step Camera" << direction; + int c = _currentCamera + direction; + if(c < 0) c = _cameras.count() - 1; + if(c >= _cameras.count()) c = 0; + setCurrentCamera(c); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stepStream(int direction) +{ + if(_lastCameraChange.elapsed() > 1000) { + _lastCameraChange.start(); + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + qCDebug(CameraManagerLog) << "Step Camera Stream" << direction; + int c = pCamera->currentStream() + direction; + if(c < 0) c = pCamera->streams()->count() - 1; + if(c >= pCamera->streams()->count()) c = 0; + pCamera->setCurrentStream(c); + } + } +} + + diff --git a/src/Camera/QGCCameraManager.h b/src/Camera/QGCCameraManager.h new file mode 100644 index 0000000..5e51501 --- /dev/null +++ b/src/Camera/QGCCameraManager.h @@ -0,0 +1,112 @@ +/*! + * @file + * @brief Camera Controller + * @author Gus Grubba + * + */ + +/// @file +/// @brief MAVLink Camera API. Camera Manager. +/// @author Gus Grubba + +#pragma once + +#include "QGCApplication.h" +#include +#include "QmlObjectListModel.h" +#include "QGCCameraControl.h" + +#include +#include + +Q_DECLARE_LOGGING_CATEGORY(CameraManagerLog) + +class Joystick; + +//----------------------------------------------------------------------------- +/// Camera Manager +class QGCCameraManager : public QObject +{ + Q_OBJECT +public: + QGCCameraManager(Vehicle* vehicle); + virtual ~QGCCameraManager(); + + Q_PROPERTY(QmlObjectListModel* cameras READ cameras NOTIFY camerasChanged) + Q_PROPERTY(QStringList cameraLabels READ cameraLabels NOTIFY cameraLabelsChanged) + Q_PROPERTY(QGCCameraControl* currentCameraInstance READ currentCameraInstance NOTIFY currentCameraChanged) + Q_PROPERTY(int currentCamera READ currentCamera WRITE setCurrentCamera NOTIFY currentCameraChanged) + + //-- Return a list of cameras provided by this vehicle + virtual QmlObjectListModel* cameras () { return &_cameras; } + //-- Camera names to show the user (for selection) + virtual QStringList cameraLabels () { return _cameraLabels; } + //-- Current selected camera + virtual int currentCamera () { return _currentCamera; } + virtual QGCCameraControl* currentCameraInstance(); + //-- Set current camera + virtual void setCurrentCamera (int sel); + //-- Current stream + virtual QGCVideoStreamInfo* currentStreamInstance(); + //-- Current thermal stream + virtual QGCVideoStreamInfo* thermalStreamInstance(); + +signals: + void camerasChanged (); + void cameraLabelsChanged (); + void currentCameraChanged (); + void streamChanged (); + +protected slots: + virtual void _vehicleReady (bool ready); + virtual void _mavlinkMessageReceived (const mavlink_message_t& message); + virtual void _activeJoystickChanged (Joystick* joystick); + virtual void _stepZoom (int direction); + virtual void _startZoom (int direction); + virtual void _stopZoom (); + virtual void _stepCamera (int direction); + virtual void _stepStream (int direction); + virtual void _cameraTimeout (); + virtual void _triggerCamera (); + virtual void _startVideoRecording (); + virtual void _stopVideoRecording (); + virtual void _toggleVideoRecording (); + +protected: + virtual QGCCameraControl* _findCamera (int id); + virtual void _requestCameraInfo (int compID); + virtual void _handleHeartbeat (const mavlink_message_t& message); + virtual void _handleCameraInfo (const mavlink_message_t& message); + virtual void _handleStorageInfo (const mavlink_message_t& message); + virtual void _handleCameraSettings (const mavlink_message_t& message); + virtual void _handleParamAck (const mavlink_message_t& message); + virtual void _handleParamValue (const mavlink_message_t& message); + virtual void _handleCaptureStatus (const mavlink_message_t& message); + virtual void _handleVideoStreamInfo (const mavlink_message_t& message); + virtual void _handleVideoStreamStatus(const mavlink_message_t& message); + virtual void _handleBatteryStatus (const mavlink_message_t& message); + +protected: + + class CameraStruct : public QObject { + public: + CameraStruct(QObject* parent, uint8_t compID_); + QElapsedTimer lastHeartbeat; + bool infoReceived = false; + bool gaveUp = false; + int tryCount = 0; + uint8_t compID = 0; + }; + + Vehicle* _vehicle = nullptr; + Joystick* _activeJoystick = nullptr; + bool _vehicleReadyState = false; + int _currentTask = 0; + QmlObjectListModel _cameras; + QStringList _cameraLabels; + int _currentCamera = 0; + QElapsedTimer _lastZoomChange; + QElapsedTimer _lastCameraChange; + QTimer _cameraTimer; + QMap _cameraInfoRequest; +}; diff --git a/README.md b/src/README.md similarity index 100% rename from README.md rename to src/README.md diff --git a/src/api/QGCCorePlugin.cc b/src/api/QGCCorePlugin.cc new file mode 100644 index 0000000..318c7a9 --- /dev/null +++ b/src/api/QGCCorePlugin.cc @@ -0,0 +1,502 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QGCApplication.h" +#include "QGCCorePlugin.h" +#include "QGCOptions.h" +#include "QmlComponentInfo.h" +#include "FactMetaData.h" +#include "SettingsManager.h" +#include "AppMessages.h" +#include "QmlObjectListModel.h" +#include "VideoManager.h" +#if defined(QGC_GST_STREAMING) +#include "GStreamer.h" +#include "VideoReceiver.h" +#endif +#include "QGCLoggingCategory.h" +#include "QGCCameraManager.h" +#include "HorizontalFactValueGrid.h" +#include "InstrumentValueData.h" + +#include +#include + +/// @file +/// @brief Core Plugin Interface for QGroundControl - Default Implementation +/// @author Gus Grubba + +class QGCCorePlugin_p +{ +public: + QGCCorePlugin_p() + { + } + + ~QGCCorePlugin_p() + { + if(pGeneral) + delete pGeneral; + if(pCommLinks) + delete pCommLinks; + if(pOfflineMaps) + delete pOfflineMaps; +#if defined(QGC_GST_TAISYNC_ENABLED) + if(pTaisync) + delete pTaisync; +#endif +#if defined(QGC_GST_MICROHARD_ENABLED) + if(pMicrohard) + delete pMicrohard; +#endif + if(pMAVLink) + delete pMAVLink; + if(pConsole) + delete pConsole; +#if defined(QT_DEBUG) + if(pMockLink) + delete pMockLink; + if(pDebug) + delete pDebug; + if(pQmlTest) + delete pQmlTest; +#endif + if(pRemoteID) + delete pRemoteID; + if(defaultOptions) + delete defaultOptions; + } + + QmlComponentInfo* pGeneral = nullptr; + QmlComponentInfo* pCommLinks = nullptr; + QmlComponentInfo* pOfflineMaps = nullptr; +#if defined(QGC_GST_TAISYNC_ENABLED) + QmlComponentInfo* pTaisync = nullptr; +#endif +#if defined(QGC_GST_MICROHARD_ENABLED) + QmlComponentInfo* pMicrohard = nullptr; +#endif + QmlComponentInfo* pMAVLink = nullptr; + QmlComponentInfo* pConsole = nullptr; + QmlComponentInfo* pHelp = nullptr; +#if defined(QT_DEBUG) + QmlComponentInfo* pMockLink = nullptr; + QmlComponentInfo* pDebug = nullptr; + QmlComponentInfo* pQmlTest = nullptr; +#endif + QmlComponentInfo* pRemoteID = nullptr; + + QGCOptions* defaultOptions = nullptr; + QVariantList settingsList; + QVariantList analyzeList; + + QmlObjectListModel _emptyCustomMapItems; +}; + +QGCCorePlugin::~QGCCorePlugin() +{ + if(_p) { + delete _p; + } +} + +QGCCorePlugin::QGCCorePlugin(QGCApplication *app, QGCToolbox* toolbox) + : QGCTool(app, toolbox) + , _showTouchAreas(false) + , _showAdvancedUI(true) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + _p = new QGCCorePlugin_p; +} + +void QGCCorePlugin::setToolbox(QGCToolbox *toolbox) +{ + QGCTool::setToolbox(toolbox); + + qmlRegisterUncreatableType ("QGroundControl", 1, 0, "QGCCorePlugin", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl", 1, 0, "QGCOptions", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl", 1, 0, "QGCFlyViewOptions", "Reference only"); +} + +QVariantList &QGCCorePlugin::settingsPages() +{ + if(!_p->pGeneral) { + _p->pGeneral = new QmlComponentInfo(tr("General"), + QUrl::fromUserInput("qrc:/qml/GeneralSettings.qml"), + QUrl::fromUserInput("qrc:/res/gear-white.svg")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pGeneral))); + _p->pCommLinks = new QmlComponentInfo(tr("Comm Links"), + QUrl::fromUserInput("qrc:/qml/LinkSettings.qml"), + QUrl::fromUserInput("qrc:/res/waves.svg")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pCommLinks))); + _p->pOfflineMaps = new QmlComponentInfo(tr("Offline Maps"), + QUrl::fromUserInput("qrc:/qml/OfflineMap.qml"), + QUrl::fromUserInput("qrc:/res/waves.svg")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pOfflineMaps))); +#if defined(QGC_GST_TAISYNC_ENABLED) + _p->pTaisync = new QmlComponentInfo(tr("Taisync"), + QUrl::fromUserInput("qrc:/qml/TaisyncSettings.qml"), + QUrl::fromUserInput("")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pTaisync))); +#endif +#if defined(QGC_GST_MICROHARD_ENABLED) + _p->pMicrohard = new QmlComponentInfo(tr("Microhard"), + QUrl::fromUserInput("qrc:/qml/MicrohardSettings.qml"), + QUrl::fromUserInput("")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pMicrohard))); +#endif + _p->pMAVLink = new QmlComponentInfo(tr("MAVLink"), + QUrl::fromUserInput("qrc:/qml/MavlinkSettings.qml"), + QUrl::fromUserInput("qrc:/res/waves.svg")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pMAVLink))); + _p->pRemoteID = new QmlComponentInfo(tr("Remote ID"), + QUrl::fromUserInput("qrc:/qml/RemoteIDSettings.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pRemoteID))); + _p->pConsole = new QmlComponentInfo(tr("Console"), + QUrl::fromUserInput("qrc:/qml/QGroundControl/Controls/AppMessages.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pConsole))); + _p->pHelp = new QmlComponentInfo(tr("Help"), + QUrl::fromUserInput("qrc:/qml/HelpSettings.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pHelp))); +#if defined(QT_DEBUG) + //-- These are always present on Debug builds + _p->pMockLink = new QmlComponentInfo(tr("Mock Link"), + QUrl::fromUserInput("qrc:/qml/MockLink.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pMockLink))); + _p->pDebug = new QmlComponentInfo(tr("Debug"), + QUrl::fromUserInput("qrc:/qml/DebugWindow.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pDebug))); + _p->pQmlTest = new QmlComponentInfo(tr("Palette Test"), + QUrl::fromUserInput("qrc:/qml/QmlTest.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pQmlTest))); +#endif + } + return _p->settingsList; +} + +QVariantList& QGCCorePlugin::analyzePages() +{ + if (!_p->analyzeList.count()) { + _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("Log Download"), QUrl::fromUserInput("qrc:/qml/LogDownloadPage.qml"), QUrl::fromUserInput("qrc:/qmlimages/LogDownloadIcon")))); +#if !defined(__mobile__) + _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("GeoTag Images"), QUrl::fromUserInput("qrc:/qml/GeoTagPage.qml"), QUrl::fromUserInput("qrc:/qmlimages/GeoTagIcon")))); +#endif + _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("MAVLink Console"), QUrl::fromUserInput("qrc:/qml/MavlinkConsolePage.qml"), QUrl::fromUserInput("qrc:/qmlimages/MavlinkConsoleIcon")))); +#if !defined(QGC_DISABLE_MAVLINK_INSPECTOR) + _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("MAVLink Inspector"),QUrl::fromUserInput("qrc:/qml/MAVLinkInspectorPage.qml"), QUrl::fromUserInput("qrc:/qmlimages/MAVLinkInspector")))); +#endif + _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("Vibration"), QUrl::fromUserInput("qrc:/qml/VibrationPage.qml"), QUrl::fromUserInput("qrc:/qmlimages/VibrationPageIcon")))); + } + return _p->analyzeList; +} + +int QGCCorePlugin::defaultSettings() +{ + return 0; +} + +QGCOptions* QGCCorePlugin::options() +{ + if (!_p->defaultOptions) { + _p->defaultOptions = new QGCOptions(this); + } + return _p->defaultOptions; +} + +bool QGCCorePlugin::overrideSettingsGroupVisibility(QString name) +{ + Q_UNUSED(name); + + // Always show all + return true; +} + +bool QGCCorePlugin::adjustSettingMetaData(const QString& settingsGroup, FactMetaData& metaData) +{ + if (settingsGroup == AppSettings::settingsGroup) { +#if !defined(QGC_ENABLE_PAIRING) + //-- If we don't support pairing, disable it. + if (metaData.name() == AppSettings::usePairingName) { + metaData.setRawDefaultValue(false); + //-- And hide the option + return false; + } +#endif + + //-- Default Palette + if (metaData.name() == AppSettings::indoorPaletteName) { + QVariant outdoorPalette; +#if defined (__mobile__) + outdoorPalette = 0; +#else + outdoorPalette = 1; +#endif + metaData.setRawDefaultValue(outdoorPalette); + return true; + } + +#if defined (__mobile__) + if (metaData.name() == AppSettings::telemetrySaveName) { + // Mobile devices have limited storage so don't turn on telemtry saving by default + metaData.setRawDefaultValue(false); + return true; + } +#endif + +#ifndef __android__ + if (metaData.name() == AppSettings::androidSaveToSDCardName) { + // This only shows on android builds + return false; + } +#endif + } + + return true; // Show setting in ui +} + +void QGCCorePlugin::setShowTouchAreas(bool show) +{ + if (show != _showTouchAreas) { + _showTouchAreas = show; + emit showTouchAreasChanged(show); + } +} + +void QGCCorePlugin::setShowAdvancedUI(bool show) +{ + if (show != _showAdvancedUI) { + _showAdvancedUI = show; + emit showAdvancedUIChanged(show); + } +} + +void QGCCorePlugin::paletteOverride(QString /*colorName*/, QGCPalette::PaletteColorInfo_t& /*colorInfo*/) +{ + +} + +QString QGCCorePlugin::showAdvancedUIMessage() const +{ + return tr("WARNING: You are about to enter Advanced Mode. " + "If used incorrectly, this may cause your vehicle to malfunction thus voiding your warranty. " + "You should do so only if instructed by customer support. " + "Are you sure you want to enable Advanced Mode?"); +} + +void QGCCorePlugin::factValueGridCreateDefaultSettings(const QString& defaultSettingsGroup) +{ + HorizontalFactValueGrid factValueGrid(defaultSettingsGroup); + + bool includeFWValues = factValueGrid.vehicleClass() == QGCMAVLink::VehicleClassFixedWing || factValueGrid.vehicleClass() == QGCMAVLink::VehicleClassVTOL || factValueGrid.vehicleClass() == QGCMAVLink::VehicleClassAirship; + + factValueGrid.setFontSize(FactValueGrid::LargeFontSize); + + factValueGrid.appendColumn(); + factValueGrid.appendColumn(); + factValueGrid.appendColumn(); + if (includeFWValues) { + factValueGrid.appendColumn(); + } + factValueGrid.appendRow(); + + int rowIndex = 0; + QmlObjectListModel* column = factValueGrid.columns()->value(0); + + InstrumentValueData* value = column->value(rowIndex++); + value->setFact("Vehicle", "AltitudeRelative"); + value->setIcon("arrow-thick-up.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(true); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "DistanceToHome"); + value->setIcon("bookmark copy 3.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(true); + + rowIndex = 0; + column = factValueGrid.columns()->value(1); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "ClimbRate"); + value->setIcon("arrow-simple-up.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(true); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "GroundSpeed"); + value->setIcon("arrow-simple-right.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(true); + + + if (includeFWValues) { + rowIndex = 0; + column = factValueGrid.columns()->value(2); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "AirSpeed"); + value->setText("AirSpd"); + value->setShowUnits(true); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "ThrottlePct"); + value->setText("Thr"); + value->setShowUnits(true); + } + + rowIndex = 0; + column = factValueGrid.columns()->value(includeFWValues ? 3 : 2); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "FlightTime"); + value->setIcon("timer.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(false); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "FlightDistance"); + value->setIcon("travel-walk.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(true); +} + +QQmlApplicationEngine* QGCCorePlugin::createQmlApplicationEngine(QObject* parent) +{ + QQmlApplicationEngine* qmlEngine = new QQmlApplicationEngine(parent); + qmlEngine->addImportPath("qrc:/qml"); + qmlEngine->rootContext()->setContextProperty("joystickManager", qgcApp()->toolbox()->joystickManager()); + qmlEngine->rootContext()->setContextProperty("debugMessageModel", AppMessages::getModel()); + return qmlEngine; +} + +void QGCCorePlugin::createRootWindow(QQmlApplicationEngine* qmlEngine) +{ + qmlEngine->load(QUrl(QStringLiteral("qrc:/qml/MainRootWindow.qml"))); +} + +bool QGCCorePlugin::mavlinkMessage(Vehicle* vehicle, LinkInterface* link, mavlink_message_t message) +{ + Q_UNUSED(vehicle); + Q_UNUSED(link); + Q_UNUSED(message); + + return true; +} + +QmlObjectListModel* QGCCorePlugin::customMapItems() +{ + return &_p->_emptyCustomMapItems; +} + +VideoManager* QGCCorePlugin::createVideoManager(QGCApplication *app, QGCToolbox *toolbox) +{ + return new VideoManager(app, toolbox); +} + +VideoReceiver* QGCCorePlugin::createVideoReceiver(QObject* parent) +{ +#if defined(QGC_GST_STREAMING) + return GStreamer::createVideoReceiver(parent); +#else + Q_UNUSED(parent) + return nullptr; +#endif +} + +void* QGCCorePlugin::createVideoSink(QObject* parent, QQuickItem* widget) +{ +#if defined(QGC_GST_STREAMING) + return GStreamer::createVideoSink(parent, widget); +#else + Q_UNUSED(parent) + Q_UNUSED(widget) + return nullptr; +#endif +} + +void QGCCorePlugin::releaseVideoSink(void* sink) +{ +#if defined(QGC_GST_STREAMING) + GStreamer::releaseVideoSink(sink); +#else + Q_UNUSED(sink) +#endif +} + +bool QGCCorePlugin::guidedActionsControllerLogging() const +{ + return GuidedActionsControllerLog().isDebugEnabled(); +} + +QString QGCCorePlugin::stableVersionCheckFileUrl() const +{ +#ifdef QGC_CUSTOM_BUILD + // Custom builds must override to turn on and provide their own location + return QString(); +#else + return QString("https://s3-us-west-2.amazonaws.com/qgroundcontrol/latest/QGC.version.txt"); +#endif +} + +const QVariantList& QGCCorePlugin::toolBarIndicators(void) +{ + //-- Default list of indicators for all vehicles. + if(_toolBarIndicatorList.size() == 0) { + _toolBarIndicatorList = QVariantList({ + QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GPSRTKIndicator.qml")), + }); + } + return _toolBarIndicatorList; +} + +QList QGCCorePlugin::firstRunPromptStdIds(void) +{ + QList rgStdIds = { unitsFirstRunPromptId, offlineVehicleFirstRunPromptId }; + return rgStdIds; +} + +QList QGCCorePlugin::firstRunPromptCustomIds(void) +{ + return QList(); +} + +QVariantList QGCCorePlugin::firstRunPromptsToShow(void) +{ + QList rgIdsToShow; + + rgIdsToShow.append(firstRunPromptStdIds()); + rgIdsToShow.append(firstRunPromptCustomIds()); + + QList rgAlreadyShownIds = AppSettings::firstRunPromptsIdsVariantToList(_toolbox->settingsManager()->appSettings()->firstRunPromptIdsShown()->rawValue()); + + for (int idToRemove: rgAlreadyShownIds) { + rgIdsToShow.removeOne(idToRemove); + } + + QVariantList rgVarIdsToShow; + for (int id: rgIdsToShow) { + rgVarIdsToShow.append(id); + } + + return rgVarIdsToShow; +} + +QString QGCCorePlugin::firstRunPromptResource(int id) +{ + switch (id) { + case unitsFirstRunPromptId: + return "/FirstRunPromptDialogs/UnitsFirstRunPrompt.qml"; + case offlineVehicleFirstRunPromptId: + return "/FirstRunPromptDialogs/OfflineVehicleFirstRunPrompt.qml"; + break; + } + + return QString(); +} diff --git a/src/api/QGCCorePlugin.h b/src/api/QGCCorePlugin.h new file mode 100644 index 0000000..791724f --- /dev/null +++ b/src/api/QGCCorePlugin.h @@ -0,0 +1,222 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "QGCToolbox.h" +#include "QGCPalette.h" +#include "QGCMAVLink.h" +#include "QmlObjectListModel.h" + +#include +#include + +/// @file +/// @brief Core Plugin Interface for QGroundControl +/// @author Gus Grubba + +class QGCApplication; +class QGCOptions; +class QGCSettings; +class QGCCorePlugin_p; +class FactMetaData; +class QGeoPositionInfoSource; +class QQmlApplicationEngine; +class Vehicle; +class LinkInterface; +class QmlObjectListModel; +class VideoReceiver; +class VideoSink; +class PlanMasterController; +class QGCCameraManager; +class QGCCameraControl; +class QQuickItem; +class InstrumentValueAreaController; + +#ifndef OPAQUE_PTR_QGCCorePlugin + #define OPAQUE_PTR_QGCCorePlugin + Q_DECLARE_OPAQUE_POINTER(QGCOptions*) +#endif + +class QGCCorePlugin : public QGCTool +{ + Q_OBJECT +public: + QGCCorePlugin(QGCApplication* app, QGCToolbox* toolbox); + ~QGCCorePlugin(); + + Q_PROPERTY(QVariantList settingsPages READ settingsPages NOTIFY settingsPagesChanged) + Q_PROPERTY(QVariantList analyzePages READ analyzePages NOTIFY analyzePagesChanged) + Q_PROPERTY(int defaultSettings READ defaultSettings CONSTANT) + Q_PROPERTY(QGCOptions* options READ options CONSTANT) + Q_PROPERTY(bool showTouchAreas READ showTouchAreas WRITE setShowTouchAreas NOTIFY showTouchAreasChanged) + Q_PROPERTY(bool showAdvancedUI READ showAdvancedUI WRITE setShowAdvancedUI NOTIFY showAdvancedUIChanged) + Q_PROPERTY(QString showAdvancedUIMessage READ showAdvancedUIMessage CONSTANT) + Q_PROPERTY(QString brandImageIndoor READ brandImageIndoor CONSTANT) + Q_PROPERTY(QString brandImageOutdoor READ brandImageOutdoor CONSTANT) + Q_PROPERTY(QmlObjectListModel* customMapItems READ customMapItems CONSTANT) + Q_PROPERTY(QVariantList toolBarIndicators READ toolBarIndicators NOTIFY toolBarIndicatorsChanged) + Q_PROPERTY(int unitsFirstRunPromptId MEMBER unitsFirstRunPromptId CONSTANT) + Q_PROPERTY(int offlineVehicleFirstRunPromptId MEMBER offlineVehicleFirstRunPromptId CONSTANT) + + Q_INVOKABLE bool guidedActionsControllerLogging() const; + + /// The list of settings under the Settings Menu + /// @return A list of QGCSettings + virtual QVariantList& settingsPages(); + + /// The list of pages/buttons under the Analyze Menu + /// @return A list of QmlPageInfo + virtual QVariantList& analyzePages(); + + /// The default settings panel to show + /// @return The settings index + virtual int defaultSettings(); + + /// Global options + /// @return An instance of QGCOptions + virtual QGCOptions* options(); + + /// Allows the core plugin to override the visibility for a settings group + /// @param name - SettingsGroup name + /// @return true: Show settings ui, false: Hide settings ui + virtual bool overrideSettingsGroupVisibility(QString name); + + /// Allows the core plugin to override the setting meta data before the setting fact is created. + /// @param settingsGroup - QSettings group which contains this item + /// @param metaData - MetaData for setting fact + /// @return true: Setting should be visible in ui, false: Setting should not be shown in ui + virtual bool adjustSettingMetaData(const QString& settingsGroup, FactMetaData& metaData); + + /// Return the resource file which contains the brand image for for Indoor theme. + virtual QString brandImageIndoor() const { return QString(); } + + /// Return the resource file which contains the brand image for for Outdoor theme. + virtual QString brandImageOutdoor() const { return QString(); } + + /// @return The message to show to the user when they a re prompted to confirm turning on advanced ui. + virtual QString showAdvancedUIMessage() const; + + /// @return An instance of an alternate position source (or NULL if not available) + virtual QGeoPositionInfoSource* createPositionSource(QObject* /*parent*/) { return nullptr; } + + /// Allows a plugin to override the specified color name from the palette + virtual void paletteOverride(QString colorName, QGCPalette::PaletteColorInfo_t& colorInfo); + + virtual void factValueGridCreateDefaultSettings(const QString& defaultSettingsGroup); + + /// Allows the plugin to override or get access to the QmlApplicationEngine to do things like add import + /// path or stuff things into the context prior to window creation. + virtual QQmlApplicationEngine* createQmlApplicationEngine(QObject* parent); + + /// Allows the plugin to override the creation of the root (native) window. + virtual void createRootWindow(QQmlApplicationEngine* qmlEngine); + + /// Allows the plugin to override the creation of VideoManager. + virtual VideoManager* createVideoManager(QGCApplication* app, QGCToolbox* toolbox); + /// Allows the plugin to override the creation of VideoReceiver. + virtual VideoReceiver* createVideoReceiver(QObject* parent); + /// Allows the plugin to override the creation of VideoSink. + virtual void* createVideoSink(QObject* parent, QQuickItem* widget); + /// Allows the plugin to override the release of VideoSink. + virtual void releaseVideoSink(void* sink); + + /// Allows the plugin to see all mavlink traffic to a vehicle + /// @return true: Allow vehicle to continue processing, false: Vehicle should not process message + virtual bool mavlinkMessage(Vehicle* vehicle, LinkInterface* link, mavlink_message_t message); + + /// Allows custom builds to add custom items to the FlightMap. Objects put into QmlObjectListModel should derive from QmlComponentInfo and set the url property. + virtual QmlObjectListModel* customMapItems(); + + /// Allows custom builds to add custom items to the plan file before the document is created. + virtual void preSaveToJson (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {} + /// Allows custom builds to add custom items to the plan file after the document is created. + virtual void postSaveToJson (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {} + + /// Allows custom builds to add custom items to the mission section of the plan file before the item is created. + virtual void preSaveToMissionJson (PlanMasterController* /*pController*/, QJsonObject& /*missionJson*/) {} + /// Allows custom builds to add custom items to the mission section of the plan file after the item is created. + virtual void postSaveToMissionJson (PlanMasterController* /*pController*/, QJsonObject& /*missionJson*/) {} + + /// Allows custom builds to load custom items from the plan file before the document is parsed. + virtual void preLoadFromJson (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {} + /// Allows custom builds to load custom items from the plan file after the document is parsed. + virtual void postLoadFromJson (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {} + + /// Returns the url to download the stable version check file. Return QString() to indicate no version check should be performed. + /// Default QGC mainline implemenentation returns QGC Stable file location. Default QGC custom build code returns QString(). + /// Custom builds can override to turn on and provide their own location. + /// The contents of this file should be a single line in the form: + /// v3.4.4 + /// This indicates the latest stable version number. + virtual QString stableVersionCheckFileUrl() const; + + /// Returns the user visible url to show user where to download new stable builds from. + /// Custom builds must override to provide their own location. + virtual QString stableDownloadLocation() const { return QString("qgroundcontrol.com"); } + + /// Returns the complex mission items to display in the Plan UI + /// @param complexMissionItemNames Default set of complex items + /// @return Complex items to be made available to user + virtual QStringList complexMissionItemNames(Vehicle* /*vehicle*/, const QStringList& complexMissionItemNames) { return complexMissionItemNames; } + + /// Returns the standard list of first run prompt ids for possible display. Actual display is based on the + /// current AppSettings::firstRunPromptIds value. The order of this list also determines the order the prompts + /// will be displayed in. + virtual QList firstRunPromptStdIds(void); + + /// Returns the custom build list of first run prompt ids for possible display. Actual display is based on the + /// current AppSettings::firstRunPromptIds value. The order of this list also determines the order the prompts + /// will be displayed in. + virtual QList firstRunPromptCustomIds(void); + + /// Returns the resource which contains the specified first run prompt for display + Q_INVOKABLE virtual QString firstRunPromptResource(int id); + + /// Returns the list of toolbar indicators which are not related to a vehicle + /// signals toolbarIndicatorsChanged + /// @return A list of QUrl with the indicators + virtual const QVariantList& toolBarIndicators(void); + + /// Returns the list of first run prompt ids which need to be displayed according to current settings + Q_INVOKABLE QVariantList firstRunPromptsToShow(void); + + bool showTouchAreas() const { return _showTouchAreas; } + bool showAdvancedUI() const { return _showAdvancedUI; } + void setShowTouchAreas(bool show); + void setShowAdvancedUI(bool show); + + // Override from QGCTool + void setToolbox (QGCToolbox* toolbox); + + // Standard first run prompt ids + static const int unitsFirstRunPromptId = 1; + static const int offlineVehicleFirstRunPromptId = 2; + + // Custom builds can start there first run prompt ids from here + static const int firstRunPromptIdsFirstCustomId = 10000; + +signals: + void settingsPagesChanged (); + void analyzePagesChanged (); + void showTouchAreasChanged (bool showTouchAreas); + void showAdvancedUIChanged (bool showAdvancedUI); + void toolBarIndicatorsChanged (); + +protected: + bool _showTouchAreas; + bool _showAdvancedUI; + Vehicle* _activeVehicle = nullptr; + QGCCameraManager* _cameraManager = nullptr; + QGCCameraControl* _currentCamera = nullptr; + QVariantList _toolBarIndicatorList; + +private: + QGCCorePlugin_p* _p; +}; diff --git a/src/api/QGCOptions.cc b/src/api/QGCOptions.cc new file mode 100644 index 0000000..2e2c05c --- /dev/null +++ b/src/api/QGCOptions.cc @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QGCOptions.h" + +#include + +/// @file +/// @brief Core Plugin Interface for QGroundControl - Application Options +/// @author Gus Grubba + +QGCOptions::QGCOptions(QObject* parent) + : QObject(parent) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); +} + +QColor QGCOptions::toolbarBackgroundLight() const +{ + return QColor(255,255,255); +} + +QColor QGCOptions::toolbarBackgroundDark() const +{ + return QColor(0,0,0); +} + +QGCFlyViewOptions* QGCOptions::flyViewOptions(void) +{ + if (!_defaultFlyViewOptions) { + _defaultFlyViewOptions = new QGCFlyViewOptions(this); + } + return _defaultFlyViewOptions; +} + +QGCFlyViewOptions::QGCFlyViewOptions(QGCOptions* options, QObject* parent) + : QObject (parent) + , _options (options) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); +} diff --git a/src/api/QGCOptions.h b/src/api/QGCOptions.h new file mode 100644 index 0000000..7b5926d --- /dev/null +++ b/src/api/QGCOptions.h @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +class QGCOptions; + +class QGCFlyViewOptions : public QObject +{ + Q_OBJECT +public: + QGCFlyViewOptions(QGCOptions* options, QObject* parent = nullptr); + + Q_PROPERTY(bool showMultiVehicleList READ showMultiVehicleList CONSTANT) + Q_PROPERTY(bool showInstrumentPanel READ showInstrumentPanel CONSTANT) + Q_PROPERTY(bool showMapScale READ showMapScale CONSTANT) + Q_PROPERTY(bool guidedBarShowEmergencyStop READ guidedBarShowEmergencyStop NOTIFY guidedBarShowEmergencyStopChanged) + Q_PROPERTY(bool guidedBarShowOrbit READ guidedBarShowOrbit NOTIFY guidedBarShowOrbitChanged) + Q_PROPERTY(bool guidedBarShowROI READ guidedBarShowROI NOTIFY guidedBarShowROIChanged) + +protected: + virtual bool showMultiVehicleList () const { return true; } + virtual bool showMapScale () const { return true; } + virtual bool showInstrumentPanel () const { return true; } + virtual bool guidedBarShowEmergencyStop () const { return true; } + virtual bool guidedBarShowOrbit () const { return true; } + virtual bool guidedBarShowROI () const { return true; } + + QGCOptions* _options; + +signals: + void guidedBarShowEmergencyStopChanged (bool show); + void guidedBarShowOrbitChanged (bool show); + void guidedBarShowROIChanged (bool show); +}; + +class QGCOptions : public QObject +{ + Q_OBJECT +public: + QGCOptions(QObject* parent = nullptr); + + Q_PROPERTY(bool combineSettingsAndSetup READ combineSettingsAndSetup CONSTANT) + Q_PROPERTY(double toolbarHeightMultiplier READ toolbarHeightMultiplier CONSTANT) + Q_PROPERTY(bool enablePlanViewSelector READ enablePlanViewSelector CONSTANT) + Q_PROPERTY(QUrl preFlightChecklistUrl READ preFlightChecklistUrl CONSTANT) + Q_PROPERTY(bool showSensorCalibrationCompass READ showSensorCalibrationCompass NOTIFY showSensorCalibrationCompassChanged) + Q_PROPERTY(bool showSensorCalibrationGyro READ showSensorCalibrationGyro NOTIFY showSensorCalibrationGyroChanged) + Q_PROPERTY(bool showSensorCalibrationAccel READ showSensorCalibrationAccel NOTIFY showSensorCalibrationAccelChanged) + Q_PROPERTY(bool showSensorCalibrationLevel READ showSensorCalibrationLevel NOTIFY showSensorCalibrationLevelChanged) + Q_PROPERTY(bool showSensorCalibrationAirspeed READ showSensorCalibrationAirspeed NOTIFY showSensorCalibrationAirspeedChanged) + Q_PROPERTY(bool sensorsHaveFixedOrientation READ sensorsHaveFixedOrientation CONSTANT) + Q_PROPERTY(bool wifiReliableForCalibration READ wifiReliableForCalibration CONSTANT) + Q_PROPERTY(bool showFirmwareUpgrade READ showFirmwareUpgrade NOTIFY showFirmwareUpgradeChanged) + Q_PROPERTY(QString firmwareUpgradeSingleURL READ firmwareUpgradeSingleURL CONSTANT) + Q_PROPERTY(bool missionWaypointsOnly READ missionWaypointsOnly NOTIFY missionWaypointsOnlyChanged) + Q_PROPERTY(bool multiVehicleEnabled READ multiVehicleEnabled NOTIFY multiVehicleEnabledChanged) + Q_PROPERTY(bool showOfflineMapExport READ showOfflineMapExport NOTIFY showOfflineMapExportChanged) + Q_PROPERTY(bool showOfflineMapImport READ showOfflineMapImport NOTIFY showOfflineMapImportChanged) + Q_PROPERTY(bool useMobileFileDialog READ useMobileFileDialog CONSTANT) + Q_PROPERTY(bool showMissionStatus READ showMissionStatus CONSTANT) + Q_PROPERTY(bool guidedActionsRequireRCRSSI READ guidedActionsRequireRCRSSI CONSTANT) + Q_PROPERTY(bool showMissionAbsoluteAltitude READ showMissionAbsoluteAltitude NOTIFY showMissionAbsoluteAltitudeChanged) + Q_PROPERTY(bool showSimpleMissionStart READ showSimpleMissionStart NOTIFY showSimpleMissionStartChanged) + Q_PROPERTY(bool disableVehicleConnection READ disableVehicleConnection CONSTANT) + Q_PROPERTY(float devicePixelRatio READ devicePixelRatio NOTIFY devicePixelRatioChanged) + Q_PROPERTY(float devicePixelDensity READ devicePixelDensity NOTIFY devicePixelDensityChanged) + Q_PROPERTY(bool checkFirmwareVersion READ checkFirmwareVersion CONSTANT) + Q_PROPERTY(bool showMavlinkLogOptions READ showMavlinkLogOptions CONSTANT) + Q_PROPERTY(bool enableSaveMainWindowPosition READ enableSaveMainWindowPosition CONSTANT) + Q_PROPERTY(QStringList surveyBuiltInPresetNames READ surveyBuiltInPresetNames CONSTANT) + Q_PROPERTY(bool allowJoystickSelection READ allowJoystickSelection NOTIFY allowJoystickSelectionChanged) + + Q_PROPERTY(QGCFlyViewOptions* flyView READ flyViewOptions CONSTANT) + + /// Should QGC hide its settings menu and colapse it into one single menu (Settings and Vehicle Setup)? + /// @return true if QGC should consolidate both menus into one. + virtual bool combineSettingsAndSetup () { return false; } + + /// Main ToolBar Multiplier. + /// @return Factor to use when computing toolbar height + virtual double toolbarHeightMultiplier () { return 1.0; } + + /// Enable Plan View Selector (Mission, Fence or Rally) + /// @return True or false + virtual bool enablePlanViewSelector () { return true; } + + /// Should the mission status indicator (Plan View) be shown? + /// @return Yes or no + virtual bool showMissionStatus () { return true; } + + /// Provides an optional, custom preflight checklist + virtual QUrl preFlightChecklistUrl () const { return QUrl::fromUserInput("qrc:/qml/PreFlightCheckList.qml"); } + + /// Allows replacing the toolbar Light Theme color + virtual QColor toolbarBackgroundLight () const; + /// Allows replacing the toolbar Dark Theme color + virtual QColor toolbarBackgroundDark () const; + /// By returning false you can hide the following sensor calibration pages + virtual bool showSensorCalibrationCompass () const { return true; } + virtual bool showSensorCalibrationGyro () const { return true; } + virtual bool showSensorCalibrationAccel () const { return true; } + virtual bool showSensorCalibrationLevel () const { return true; } + virtual bool showSensorCalibrationAirspeed () const { return true; } + virtual bool wifiReliableForCalibration () const { return false; } + virtual bool sensorsHaveFixedOrientation () const { return false; } + virtual bool showFirmwareUpgrade () const { return true; } + virtual bool missionWaypointsOnly () const { return false; } ///< true: Only allow waypoints and complex items in Plan + virtual bool multiVehicleEnabled () const { return true; } ///< false: multi vehicle support is disabled + virtual bool guidedActionsRequireRCRSSI () const { return false; } ///< true: Guided actions will be disabled is there is no RC RSSI + virtual bool showOfflineMapExport () const { return true; } + virtual bool showOfflineMapImport () const { return true; } + virtual bool showMissionAbsoluteAltitude () const { return true; } + virtual bool showSimpleMissionStart () const { return false; } + virtual bool disableVehicleConnection () const { return false; } ///< true: vehicle connection is disabled + virtual bool checkFirmwareVersion () const { return true; } + virtual bool showMavlinkLogOptions () const { return true; } + virtual bool allowJoystickSelection () const { return true; } ///< false: custom build has automatically enabled a specific joystick + /// Desktop builds save the main application size and position on close (and restore it on open) + virtual bool enableSaveMainWindowPosition () const { return true; } + virtual QStringList surveyBuiltInPresetNames () const { return QStringList(); } // Built in presets cannot be deleted + +#if defined(__mobile__) + virtual bool useMobileFileDialog () const { return true;} +#else + virtual bool useMobileFileDialog () const { return false;} +#endif + + /// If returned QString in non-empty it means that firmware upgrade will run in a mode which only + /// supports downloading a single firmware file from the URL. It also supports custom install through + /// the Advanced options. + virtual QString firmwareUpgradeSingleURL () const { return QString(); } + + /// Device specific pixel ratio/density (for when Qt doesn't properly read it from the hardware) + virtual float devicePixelRatio () const { return 0.0f; } + virtual float devicePixelDensity () const { return 0.0f; } + + virtual QGCFlyViewOptions* flyViewOptions (); + +signals: + void showSensorCalibrationCompassChanged (bool show); + void showSensorCalibrationGyroChanged (bool show); + void showSensorCalibrationAccelChanged (bool show); + void showSensorCalibrationLevelChanged (bool show); + void showSensorCalibrationAirspeedChanged (bool show); + void showFirmwareUpgradeChanged (bool show); + void missionWaypointsOnlyChanged (bool missionWaypointsOnly); + void multiVehicleEnabledChanged (bool multiVehicleEnabled); + void allowJoystickSelectionChanged (bool allow); + void showOfflineMapExportChanged (); + void showOfflineMapImportChanged (); + void showMissionAbsoluteAltitudeChanged (); + void showSimpleMissionStartChanged (); + void devicePixelRatioChanged (); + void devicePixelDensityChanged (); + +protected: + QGCFlyViewOptions* _defaultFlyViewOptions = nullptr; +}; diff --git a/src/api/QGCSettings.cc b/src/api/QGCSettings.cc new file mode 100644 index 0000000..c296d64 --- /dev/null +++ b/src/api/QGCSettings.cc @@ -0,0 +1,21 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QGCSettings.h" + +/// @file +/// @brief Core Plugin Interface for QGroundControl. Settings element. +/// @author Gus Grubba + +QGCSettings::QGCSettings(QString title, QUrl url, QUrl icon) + : _title(title) + , _url(url) + , _icon(icon) +{ +} diff --git a/src/api/QGCSettings.h b/src/api/QGCSettings.h new file mode 100644 index 0000000..593a178 --- /dev/null +++ b/src/api/QGCSettings.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +/// @file +/// @brief Core Plugin Interface for QGroundControl. Settings element. +/// @author Gus Grubba + +class QGCSettings : public QObject +{ + Q_OBJECT +public: + QGCSettings(QString title, QUrl url, QUrl icon = QUrl()); + + Q_PROPERTY(QString title READ title CONSTANT) + Q_PROPERTY(QUrl url READ url CONSTANT) + Q_PROPERTY(QUrl icon READ icon CONSTANT) + + virtual QString title () { return _title; } + virtual QUrl url () { return _url; } + virtual QUrl icon () { return _icon; } + +protected: + QString _title; + QUrl _url; + QUrl _icon; +}; diff --git a/src/api/QmlComponentInfo.cc b/src/api/QmlComponentInfo.cc new file mode 100644 index 0000000..35bb33c --- /dev/null +++ b/src/api/QmlComponentInfo.cc @@ -0,0 +1,19 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QmlComponentInfo.h" + +QmlComponentInfo::QmlComponentInfo(QString title, QUrl url, QUrl icon, QObject* parent) + : QObject (parent) + , _title (title) + , _url (url) + , _icon (icon) +{ + +} diff --git a/src/api/QmlComponentInfo.h b/src/api/QmlComponentInfo.h new file mode 100644 index 0000000..30c0459 --- /dev/null +++ b/src/api/QmlComponentInfo.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +/// Represents a Qml component which can be loaded from a resource. +class QmlComponentInfo : public QObject +{ + Q_OBJECT + +public: + QmlComponentInfo(QString title, QUrl url, QUrl icon = QUrl(), QObject* parent = nullptr); + + Q_PROPERTY(QString title READ title CONSTANT) ///< Title for page + Q_PROPERTY(QUrl url READ url CONSTANT) ///< Qml source code + Q_PROPERTY(QUrl icon READ icon CONSTANT) ///< Icon for page + + virtual QString title () { return _title; } + virtual QUrl url () { return _url; } + virtual QUrl icon () { return _icon; } + +protected: + QString _title; + QUrl _url; + QUrl _icon; +}; From 341e25dd77aba145f2072072a4c6a6c66a151ea3 Mon Sep 17 00:00:00 2001 From: laptoy <2107884590@qq.com> Date: Tue, 11 Jun 2024 14:53:16 +0800 Subject: [PATCH 7/9] add --- src/Camera/QGCCameraControl.cc | 2259 ++++++++++++++++++++++++++++++++ src/Camera/QGCCameraControl.h | 422 ++++++ src/Camera/QGCCameraIO.cc | 372 ++++++ src/Camera/QGCCameraIO.h | 72 + src/Camera/QGCCameraManager.cc | 518 ++++++++ src/Camera/QGCCameraManager.h | 112 ++ src/QGCCameraControl.cc | 2259 ++++++++++++++++++++++++++++++++ README.md => src/README.md | 0 src/api/QGCCorePlugin.cc | 502 +++++++ src/api/QGCCorePlugin.h | 222 ++++ src/api/QGCOptions.cc | 47 + src/api/QGCOptions.h | 170 +++ src/api/QGCSettings.cc | 21 + src/api/QGCSettings.h | 37 + src/api/QmlComponentInfo.cc | 19 + src/api/QmlComponentInfo.h | 35 + 16 files changed, 7067 insertions(+) create mode 100644 src/Camera/QGCCameraControl.cc create mode 100644 src/Camera/QGCCameraControl.h create mode 100644 src/Camera/QGCCameraIO.cc create mode 100644 src/Camera/QGCCameraIO.h create mode 100644 src/Camera/QGCCameraManager.cc create mode 100644 src/Camera/QGCCameraManager.h create mode 100644 src/QGCCameraControl.cc rename README.md => src/README.md (100%) create mode 100644 src/api/QGCCorePlugin.cc create mode 100644 src/api/QGCCorePlugin.h create mode 100644 src/api/QGCOptions.cc create mode 100644 src/api/QGCOptions.h create mode 100644 src/api/QGCSettings.cc create mode 100644 src/api/QGCSettings.h create mode 100644 src/api/QmlComponentInfo.cc create mode 100644 src/api/QmlComponentInfo.h diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc new file mode 100644 index 0000000..f85acb7 --- /dev/null +++ b/src/Camera/QGCCameraControl.cc @@ -0,0 +1,2259 @@ + + +#include "QGCCameraControl.h" +#include "QGCCameraIO.h" +#include "SettingsManager.h" +#include "VideoManager.h" +#include "QGCMapEngine.h" +#include "QGCCameraManager.h" +#include "FTPManager.h" +#include "QGCLZMA.h" + +#include +#include +#include +#include + +QGC_LOGGING_CATEGORY(CameraControlLog, "CameraControlLog") +QGC_LOGGING_CATEGORY(CameraControlVerboseLog, "CameraControlVerboseLog") + +static const char* kCondition = "condition"; +static const char* kControl = "control"; +static const char* kDefault = "default"; +static const char* kDefnition = "definition"; +static const char* kDescription = "description"; +static const char* kExclusion = "exclude"; +static const char* kExclusions = "exclusions"; +static const char* kLocale = "locale"; +static const char* kLocalization = "localization"; +static const char* kMax = "max"; +static const char* kMin = "min"; +static const char* kModel = "model"; +static const char* kName = "name"; +static const char* kOption = "option"; +static const char* kOptions = "options"; +static const char* kOriginal = "original"; +static const char* kParameter = "parameter"; +static const char* kParameterrange = "parameterrange"; +static const char* kParameterranges = "parameterranges"; +static const char* kParameters = "parameters"; +static const char* kReadOnly = "readonly"; +static const char* kWriteOnly = "writeonly"; +static const char* kRoption = "roption"; +static const char* kStep = "step"; +static const char* kDecimalPlaces = "decimalPlaces"; +static const char* kStrings = "strings"; +static const char* kTranslated = "translated"; +static const char* kType = "type"; +static const char* kUnit = "unit"; +static const char* kUpdate = "update"; +static const char* kUpdates = "updates"; +static const char* kValue = "value"; +static const char* kVendor = "vendor"; +static const char* kVersion = "version"; + +static const char* kPhotoMode = "PhotoMode"; +static const char* kPhotoLapse = "PhotoLapse"; +static const char* kPhotoLapseCount = "PhotoLapseCount"; +static const char* kThermalOpacity = "ThermalOpacity"; +static const char* kThermalMode = "ThermalMode"; + +//----------------------------------------------------------------------------- +// Known Parameters +const char* QGCCameraControl::kCAM_EV = "CAM_EV"; +const char* QGCCameraControl::kCAM_EXPMODE = "CAM_EXPMODE"; +const char* QGCCameraControl::kCAM_ISO = "CAM_ISO"; +const char* QGCCameraControl::kCAM_SHUTTERSPD = "CAM_SHUTTERSPD"; +const char* QGCCameraControl::kCAM_APERTURE = "CAM_APERTURE"; +const char* QGCCameraControl::kCAM_WBMODE = "CAM_WBMODE"; +const char* QGCCameraControl::kCAM_MODE = "CAM_MODE"; + +//----------------------------------------------------------------------------- +QGCCameraOptionExclusion::QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_) + : QObject(parent) + , param(param_) + , value(value_) + , exclusions(exclusions_) +{ +} + +//----------------------------------------------------------------------------- +QGCCameraOptionRange::QGCCameraOptionRange(QObject* parent, QString param_, QString value_, QString targetParam_, QString condition_, QStringList optNames_, QStringList optValues_) + : QObject(parent) + , param(param_) + , value(value_) + , targetParam(targetParam_) + , condition(condition_) + , optNames(optNames_) + , optValues(optValues_) +{ +} + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, bool& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue() != "0"; + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, int& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue().toInt(); + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, QString& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue(); + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_value(QDomNode& element, const char* tagName, QString& target) +{ + QDomElement de = element.firstChildElement(tagName); + if(de.isNull()) { + return false; + } + target = de.text(); + return true; +} + +//----------------------------------------------------------------------------- +QGCCameraControl::QGCCameraControl(const mavlink_camera_information_t *info, Vehicle* vehicle, int compID, QObject* parent) + : FactGroup(0, parent, true /* ignore camel case */) + , _vehicle(vehicle) + , _compID(compID) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + memcpy(&_info, info, sizeof(mavlink_camera_information_t)); + connect(this, &QGCCameraControl::dataReady, this, &QGCCameraControl::_dataReady); + _vendor = QString(reinterpret_cast(info->vendor_name)); + _modelName = QString(reinterpret_cast(info->model_name)); + int ver = static_cast(_info.cam_definition_version); + _cacheFile = QString::asprintf("%s/%s_%s_%03d.xml", + qgcApp()->toolbox()->settingsManager()->appSettings()->parameterSavePath().toStdString().c_str(), + _vendor.toStdString().c_str(), + _modelName.toStdString().c_str(), + ver); + if(info->cam_definition_uri[0] != 0) { + //-- Process camera definition file + _handleDefinitionFile(info->cam_definition_uri); + } else { + _initWhenReady(); + } + QSettings settings; + _photoMode = static_cast(settings.value(kPhotoMode, static_cast(PHOTO_CAPTURE_SINGLE)).toInt()); + _photoLapse = settings.value(kPhotoLapse, 1.0).toDouble(); + _photoLapseCount = settings.value(kPhotoLapseCount, 0).toInt(); + _thermalOpacity = settings.value(kThermalOpacity, 85.0).toDouble(); + _thermalMode = static_cast(settings.value(kThermalMode, static_cast(THERMAL_BLEND)).toUInt()); + _recTimer.setSingleShot(false); + _recTimer.setInterval(333); + connect(&_recTimer, &QTimer::timeout, this, &QGCCameraControl::_recTimerHandler); +} + +//----------------------------------------------------------------------------- +QGCCameraControl::~QGCCameraControl() +{ + delete _netManager; + _netManager = nullptr; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_initWhenReady() +{ + qCDebug(CameraControlLog) << "_initWhenReady()"; + if(isBasic()) { + qCDebug(CameraControlLog) << "Basic, MAVLink only messages."; + _requestCameraSettings(); + QTimer::singleShot(250, this, &QGCCameraControl::_checkForVideoStreams); + //-- Basic cameras have no parameters + _paramComplete = true; + emit parametersReady(); + } else { + _requestAllParameters(); + //-- Give some time to load the parameters before going after the camera settings + QTimer::singleShot(2000, this, &QGCCameraControl::_requestCameraSettings); + } + connect(_vehicle, &Vehicle::mavCommandResult, this, &QGCCameraControl::_mavCommandResult); + connect(&_captureStatusTimer, &QTimer::timeout, this, &QGCCameraControl::_requestCaptureStatus); + _captureStatusTimer.setSingleShot(true); + QTimer::singleShot(2500, this, &QGCCameraControl::_requestStorageInfo); + _captureStatusTimer.start(2750); + emit infoChanged(); + + delete _netManager; + _netManager = nullptr; +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::firmwareVersion() +{ + int major = (_info.firmware_version >> 24) & 0xFF; + int minor = (_info.firmware_version >> 16) & 0xFF; + int build = _info.firmware_version & 0xFFFF; + return QString::asprintf("%d.%d.%d", major, minor, build); +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::recordTimeStr() +{ + return QTime(0, 0).addMSecs(static_cast(recordTime())).toString("hh:mm:ss"); +} + +//----------------------------------------------------------------------------- +QGCCameraControl::VideoStatus +QGCCameraControl::videoStatus() +{ + return _video_status; +} + +//----------------------------------------------------------------------------- +QGCCameraControl::PhotoStatus +QGCCameraControl::photoStatus() +{ + return _photo_status; +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::storageFreeStr() +{ + return QGCMapEngine::storageFreeSizeToString(static_cast(_storageFree)); +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::batteryRemainingStr() +{ + if(_batteryRemaining >= 0) { + return QGCMapEngine::numberToString(static_cast(_batteryRemaining)) + " %"; + } + return ""; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setCameraMode(CameraMode mode) +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "setCameraMode(" << mode << ")"; + if(mode == CAM_MODE_VIDEO) { + setVideoMode(); + } else if(mode == CAM_MODE_PHOTO) { + setPhotoMode(); + } else { + qCDebug(CameraControlLog) << "setCameraMode() Invalid mode:" << mode; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoMode(PhotoMode mode) +{ + if(!_resetting) { + _photoMode = mode; + QSettings settings; + settings.setValue(kPhotoMode, static_cast(mode)); + emit photoModeChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoLapse(qreal interval) +{ + _photoLapse = interval; + QSettings settings; + settings.setValue(kPhotoLapse, interval); + emit photoLapseChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoLapseCount(int count) +{ + _photoLapseCount = count; + QSettings settings; + settings.setValue(kPhotoLapseCount, count); + emit photoLapseCountChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_setCameraMode(CameraMode mode) +{ + if(_cameraMode != mode) { + _cameraMode = mode; + emit cameraModeChanged(); + //-- Update stream status + _streamStatusTimer.start(1000); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::toggleMode() +{ + if(!_resetting) { + if(cameraMode() == CAM_MODE_PHOTO || cameraMode() == CAM_MODE_SURVEY) { + setVideoMode(); + } else if(cameraMode() == CAM_MODE_VIDEO) { + setPhotoMode(); + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::toggleVideo() +{ + if(!_resetting) { + if(videoStatus() == VIDEO_CAPTURE_STATUS_RUNNING) { + return stopVideo(); + } else { + return startVideo(); + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::takePhoto() +{ + qCDebug(CameraControlLog) << "takePhoto()"; + //-- Check if camera can capture photos or if it can capture it while in Video Mode + if(!capturesPhotos()) { + qCWarning(CameraControlLog) << "Camera does not handle image capture"; + return false; + } + if(cameraMode() == CAM_MODE_VIDEO && !photosInVideoMode()) { + qCWarning(CameraControlLog) << "Camera does not handle image capture while in video mode"; + return false; + } + if(photoStatus() != PHOTO_CAPTURE_IDLE) { + qCWarning(CameraControlLog) << "Camera not idle"; + return false; + } + if(!_resetting) { + if(capturesPhotos()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_IMAGE_START_CAPTURE, // Command id + false, // ShowError + 0, // Reserved (Set to 0) + static_cast(_photoMode == PHOTO_CAPTURE_SINGLE ? 0 : _photoLapse), // Duration between two consecutive pictures (in seconds--ignored if single image) + _photoMode == PHOTO_CAPTURE_SINGLE ? 1 : _photoLapseCount); // Number of images to capture total - 0 for unlimited capture + _setPhotoStatus(PHOTO_CAPTURE_IN_PROGRESS); + _captureInfoRetries = 0; + //-- Capture local image as well + if(qgcApp()->toolbox()->videoManager()) { + qgcApp()->toolbox()->videoManager()->grabImage(); + } + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::stopTakePhoto() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "stopTakePhoto()"; + if(photoStatus() == PHOTO_CAPTURE_IDLE || (photoStatus() != PHOTO_CAPTURE_INTERVAL_IDLE && photoStatus() != PHOTO_CAPTURE_INTERVAL_IN_PROGRESS)) { + return false; + } + if(capturesPhotos()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_IMAGE_STOP_CAPTURE, // Command id + false, // ShowError + 0); // Reserved (Set to 0) + _setPhotoStatus(PHOTO_CAPTURE_IDLE); + _captureInfoRetries = 0; + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::startVideo() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "startVideo()"; + //-- Check if camera can capture videos or if it can capture it while in Photo Mode + if(!capturesVideo() || (cameraMode() == CAM_MODE_PHOTO && !videoInPhotoMode())) { + return false; + } + if(videoStatus() != VIDEO_CAPTURE_STATUS_RUNNING) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_START_CAPTURE, // Command id + false, // Don't Show Error (handle locally) + 0, // Reserved (Set to 0) + 0); // CAMERA_CAPTURE_STATUS Frequency + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::stopVideo() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "stopVideo()"; + if(videoStatus() == VIDEO_CAPTURE_STATUS_RUNNING) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_STOP_CAPTURE, // Command id + false, // Don't Show Error (handle locally) + 0); // Reserved (Set to 0) + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setVideoMode() +{ + if(!_resetting && hasModes()) { + qCDebug(CameraControlLog) << "setVideoMode()"; + //-- Does it have a mode parameter? + Fact* pMode = mode(); + if(pMode) { + if(cameraMode() != CAM_MODE_VIDEO) { + pMode->setRawValue(CAM_MODE_VIDEO); + _setCameraMode(CAM_MODE_VIDEO); + } + } else { + //-- Use MAVLink Command + if(_cameraMode != CAM_MODE_VIDEO) { + //-- Use basic MAVLink message + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_MODE, // Command id + true, // ShowError + 0, // Reserved (Set to 0) + CAM_MODE_VIDEO); // Camera mode (0: photo, 1: video) + _setCameraMode(CAM_MODE_VIDEO); + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoMode() +{ + if(!_resetting && hasModes()) { + qCDebug(CameraControlLog) << "setPhotoMode()"; + //-- Does it have a mode parameter? + Fact* pMode = mode(); + if(pMode) { + if(cameraMode() != CAM_MODE_PHOTO) { + pMode->setRawValue(CAM_MODE_PHOTO); + _setCameraMode(CAM_MODE_PHOTO); + } + } else { + //-- Use MAVLink Command + if(_cameraMode != CAM_MODE_PHOTO) { + //-- Use basic MAVLink message + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_MODE, // Command id + true, // ShowError + 0, // Reserved (Set to 0) + CAM_MODE_PHOTO); // Camera mode (0: photo, 1: video) + _setCameraMode(CAM_MODE_PHOTO); + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setThermalMode(ThermalViewMode mode) +{ + QSettings settings; + settings.setValue(kThermalMode, static_cast(mode)); + _thermalMode = mode; + emit thermalModeChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setThermalOpacity(double val) +{ + if(val < 0.0) val = 0.0; + if(val > 100.0) val = 100.0; + if(fabs(_thermalOpacity - val) > 0.1) { + _thermalOpacity = val; + QSettings settings; + settings.setValue(kThermalOpacity, val); + emit thermalOpacityChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setZoomLevel(qreal level) +{ + qCDebug(CameraControlLog) << "setZoomLevel()" << level; + if(hasZoom()) { + //-- Limit + level = std::min(std::max(level, 0.0), 100.0); + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_RANGE, // Zoom type + static_cast(level)); // Level + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setFocusLevel(qreal level) +{ + qCDebug(CameraControlLog) << "setFocusLevel()" << level; + if(hasFocus()) { + //-- Limit + level = std::min(std::max(level, 0.0), 100.0); + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_FOCUS, // Command id + false, // ShowError + FOCUS_TYPE_RANGE, // Focus type + static_cast(level)); // Level + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::resetSettings() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "resetSettings()"; + _resetting = true; + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_RESET_CAMERA_SETTINGS, // Command id + true, // ShowError + 1); // Do Reset + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::formatCard(int id) +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "formatCard()"; + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_STORAGE_FORMAT, // Command id + true, // ShowError + id, // Storage ID (1 for first, 2 for second, etc.) + 1); // Do Format + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::stepZoom(int direction) +{ + qCDebug(CameraControlLog) << "stepZoom()" << direction; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_STEP, // Zoom type + direction); // Direction (-1 wide, 1 tele) + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::startZoom(int direction) +{ + qCDebug(CameraControlLog) << "startZoom()" << direction; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_CONTINUOUS, // Zoom type + direction); // Direction (-1 wide, 1 tele) + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::stopZoom() +{ + qCDebug(CameraControlLog) << "stopZoom()"; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_CONTINUOUS, // Zoom type + 0); // Direction (-1 wide, 1 tele) + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestCaptureStatus() +{ + qCDebug(CameraControlLog) << "_requestCaptureStatus()"; + _vehicle->sendMavCommand( + _compID, // target component + MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS, // command id + false, // showError + 1); // Do Request +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::factChanged(Fact* pFact) +{ + _updateActiveList(); + _updateRanges(pFact); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle) +{ + //-- Is this ours? + if(_vehicle->id() != vehicleId || compID() != component) { + return; + } + if(!noReponseFromVehicle && result == MAV_RESULT_IN_PROGRESS) { + //-- Do Nothing + qCDebug(CameraControlLog) << "In progress response for" << command; + }else if(!noReponseFromVehicle && result == MAV_RESULT_ACCEPTED) { + switch(command) { + case MAV_CMD_RESET_CAMERA_SETTINGS: + _resetting = false; + if(isBasic()) { + _requestCameraSettings(); + } else { + QTimer::singleShot(500, this, &QGCCameraControl::_requestAllParameters); + QTimer::singleShot(2500, this, &QGCCameraControl::_requestCameraSettings); + } + break; + case MAV_CMD_VIDEO_START_CAPTURE: + _setVideoStatus(VIDEO_CAPTURE_STATUS_RUNNING); + _captureStatusTimer.start(1000); + break; + case MAV_CMD_VIDEO_STOP_CAPTURE: + _setVideoStatus(VIDEO_CAPTURE_STATUS_STOPPED); + _captureStatusTimer.start(1000); + break; + case MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS: + _captureInfoRetries = 0; + break; + case MAV_CMD_REQUEST_STORAGE_INFORMATION: + _storageInfoRetries = 0; + break; + case MAV_CMD_IMAGE_START_CAPTURE: + _captureStatusTimer.start(1000); + break; + } + } else { + if(noReponseFromVehicle || result == MAV_RESULT_TEMPORARILY_REJECTED || result == MAV_RESULT_FAILED) { + if(noReponseFromVehicle) { + qCDebug(CameraControlLog) << "No response for" << command; + } else if (result == MAV_RESULT_TEMPORARILY_REJECTED) { + qCDebug(CameraControlLog) << "Command temporarily rejected for" << command; + } else { + qCDebug(CameraControlLog) << "Command failed for" << command; + } + switch(command) { + case MAV_CMD_RESET_CAMERA_SETTINGS: + _resetting = false; + qCDebug(CameraControlLog) << "Failed to reset camera settings"; + break; + case MAV_CMD_IMAGE_START_CAPTURE: + case MAV_CMD_IMAGE_STOP_CAPTURE: + if(++_captureInfoRetries < 3) { + _captureStatusTimer.start(1000); + } else { + qCDebug(CameraControlLog) << "Giving up start/stop image capture"; + _setPhotoStatus(PHOTO_CAPTURE_IDLE); + } + break; + case MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS: + if(++_captureInfoRetries < 3) { + _captureStatusTimer.start(500); + } else { + qCDebug(CameraControlLog) << "Giving up requesting capture status"; + } + break; + case MAV_CMD_REQUEST_STORAGE_INFORMATION: + if(++_storageInfoRetries < 3) { + QTimer::singleShot(500, this, &QGCCameraControl::_requestStorageInfo); + } else { + qCDebug(CameraControlLog) << "Giving up requesting storage status"; + } + break; + } + } else { + qCDebug(CameraControlLog) << "Bad response for" << command << result; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_setVideoStatus(VideoStatus status) +{ + if(_video_status != status) { + _video_status = status; + emit videoStatusChanged(); + if(status == VIDEO_CAPTURE_STATUS_RUNNING) { + _recordTime = 0; + _recTime = QTime::currentTime(); + _recTimer.start(); + } else { + _recTimer.stop(); + _recordTime = 0; + emit recordTimeChanged(); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_recTimerHandler() +{ + _recordTime = static_cast(_recTime.msecsTo(QTime::currentTime())); + emit recordTimeChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_setPhotoStatus(PhotoStatus status) +{ + if(_photo_status != status) { + qCDebug(CameraControlLog) << "Set Photo Status:" << status; + _photo_status = status; + emit photoStatusChanged(); + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadCameraDefinitionFile(QByteArray& bytes) +{ + QByteArray originalData(bytes); + //-- Handle localization + if(!_handleLocalization(bytes)) { + return false; + } + int errorLine; + QString errorMsg; + QDomDocument doc; + if(!doc.setContent(bytes, false, &errorMsg, &errorLine)) { + qCCritical(CameraControlLog) << "Unable to parse camera definition file on line:" << errorLine; + qCCritical(CameraControlLog) << errorMsg; + return false; + } + //-- Load camera constants + QDomNodeList defElements = doc.elementsByTagName(kDefnition); + if(!defElements.size() || !_loadConstants(defElements)) { + qCWarning(CameraControlLog) << "Unable to load camera constants from camera definition"; + return false; + } + //-- Load camera parameters + QDomNodeList paramElements = doc.elementsByTagName(kParameters); + if(!paramElements.size()) { + qCDebug(CameraControlLog) << "No parameters to load from camera"; + return false; + } + if(!_loadSettings(paramElements)) { + qCWarning(CameraControlLog) << "Unable to load camera parameters from camera definition"; + return false; + } + //-- If this is new, cache it + if(!_cached) { + qCDebug(CameraControlLog) << "Saving camera definition file" << _cacheFile; + QFile file(_cacheFile); + if (!file.open(QIODevice::WriteOnly)) { + qWarning() << QString("Could not save cache file %1. Error: %2").arg(_cacheFile).arg(file.errorString()); + } else { + file.write(originalData); + } + } + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadConstants(const QDomNodeList nodeList) +{ + QDomNode node = nodeList.item(0); + if(!read_attribute(node, kVersion, _version)) { + return false; + } + if(!read_value(node, kModel, _modelName)) { + return false; + } + if(!read_value(node, kVendor, _vendor)) { + return false; + } + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadSettings(const QDomNodeList nodeList) +{ + QDomNode node = nodeList.item(0); + QDomElement elem = node.toElement(); + QDomNodeList parameters = elem.elementsByTagName(kParameter); + //-- Pre-process settings (maintain order and skip non-controls) + for(int i = 0; i < parameters.size(); i++) { + QDomNode parameterNode = parameters.item(i); + QString name; + if(read_attribute(parameterNode, kName, name)) { + bool control = true; + read_attribute(parameterNode, kControl, control); + if(control) { + _settings << name; + } + } else { + qCritical() << "Parameter entry missing parameter name"; + return false; + } + } + //-- Load parameters + for(int i = 0; i < parameters.size(); i++) { + QDomNode parameterNode = parameters.item(i); + QString factName; + read_attribute(parameterNode, kName, factName); + QString type; + if(!read_attribute(parameterNode, kType, type)) { + qCritical() << QString("Parameter %1 missing parameter type").arg(factName); + return false; + } + //-- Does it have a control? + bool control = true; + read_attribute(parameterNode, kControl, control); + //-- Is it read only? + bool readOnly = false; + read_attribute(parameterNode, kReadOnly, readOnly); + //-- Is it write only? + bool writeOnly = false; + read_attribute(parameterNode, kWriteOnly, writeOnly); + //-- It can't be both + if(readOnly && writeOnly) { + qCritical() << QString("Parameter %1 cannot be both read only and write only").arg(factName); + } + //-- Param type + bool unknownType; + FactMetaData::ValueType_t factType = FactMetaData::stringToType(type, unknownType); + if (unknownType) { + qCritical() << QString("Unknown type for parameter %1").arg(factName); + return false; + } + //-- By definition, custom types do not have control + if(factType == FactMetaData::valueTypeCustom) { + control = false; + } + //-- Description + QString description; + if(!read_value(parameterNode, kDescription, description)) { + qCritical() << QString("Parameter %1 missing parameter description").arg(factName); + return false; + } + //-- Check for updates + QStringList updates = _loadUpdates(parameterNode); + if(updates.size()) { + qCDebug(CameraControlVerboseLog) << "Parameter" << factName << "requires updates for:" << updates; + _requestUpdates[factName] = updates; + } + //-- Build metadata + FactMetaData* metaData = new FactMetaData(factType, factName, this); + QQmlEngine::setObjectOwnership(metaData, QQmlEngine::CppOwnership); + metaData->setShortDescription(description); + metaData->setLongDescription(description); + metaData->setHasControl(control); + metaData->setReadOnly(readOnly); + metaData->setWriteOnly(writeOnly); + //-- Options (enums) + QDomElement optionElem = parameterNode.toElement(); + QDomNodeList optionsRoot = optionElem.elementsByTagName(kOptions); + if(optionsRoot.size()) { + //-- Iterate options + QDomNode node = optionsRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList options = elem.elementsByTagName(kOption); + for(int i = 0; i < options.size(); i++) { + QDomNode option = options.item(i); + QString optName; + QString optValue; + QVariant optVariant; + if(!_loadNameValue(option, factName, metaData, optName, optValue, optVariant)) { + delete metaData; + return false; + } + metaData->addEnumInfo(optName, optVariant); + _originalOptNames[factName] << optName; + _originalOptValues[factName] << optVariant; + //-- Check for exclusions + QStringList exclusions = _loadExclusions(option); + if(exclusions.size()) { + qCDebug(CameraControlVerboseLog) << "New exclusions:" << factName << optValue << exclusions; + QGCCameraOptionExclusion* pExc = new QGCCameraOptionExclusion(this, factName, optValue, exclusions); + QQmlEngine::setObjectOwnership(pExc, QQmlEngine::CppOwnership); + _valueExclusions.append(pExc); + } + //-- Check for range rules + if(!_loadRanges(option, factName, optValue)) { + delete metaData; + return false; + } + } + } + QString defaultValue; + if(read_attribute(parameterNode, kDefault, defaultValue)) { + QVariant defaultVariant; + QString errorString; + if (metaData->convertAndValidateRaw(defaultValue, false, defaultVariant, errorString)) { + metaData->setRawDefaultValue(defaultVariant); + } else { + qWarning() << "Invalid default value for" << factName + << " type:" << metaData->type() + << " value:" << defaultValue + << " error:" << errorString; + } + } + //-- Set metadata and Fact + if (_nameToFactMetaDataMap.contains(factName)) { + qWarning() << QStringLiteral("Duplicate fact name:") << factName; + delete metaData; + } else { + { + //-- Check for Min Value + QString attr; + if(read_attribute(parameterNode, kMin, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setRawMin(typedValue); + } else { + qWarning() << "Invalid min value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Max Value + QString attr; + if(read_attribute(parameterNode, kMax, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setRawMax(typedValue); + } else { + qWarning() << "Invalid max value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Step Value + QString attr; + if(read_attribute(parameterNode, kStep, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setRawIncrement(typedValue.toDouble()); + } else { + qWarning() << "Invalid step value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Decimal Places + QString attr; + if(read_attribute(parameterNode, kDecimalPlaces, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setDecimalPlaces(typedValue.toInt()); + } else { + qWarning() << "Invalid decimal places value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Units + QString attr; + if(read_attribute(parameterNode, kUnit, attr)) { + metaData->setRawUnits(attr); + } + } + qCDebug(CameraControlLog) << "New parameter:" << factName << (readOnly ? "ReadOnly" : "Writable") << (writeOnly ? "WriteOnly" : "Readable"); + _nameToFactMetaDataMap[factName] = metaData; + Fact* pFact = new Fact(_compID, factName, factType, this); + QQmlEngine::setObjectOwnership(pFact, QQmlEngine::CppOwnership); + pFact->setMetaData(metaData); + pFact->_containerSetRawValue(metaData->rawDefaultValue()); + QGCCameraParamIO* pIO = new QGCCameraParamIO(this, pFact, _vehicle); + QQmlEngine::setObjectOwnership(pIO, QQmlEngine::CppOwnership); + _paramIO[factName] = pIO; + _addFact(pFact, factName); + } + } + if(_nameToFactMetaDataMap.size() > 0) { + _addFactGroup(this, "camera"); + _processRanges(); + _activeSettings = _settings; + emit activeSettingsChanged(); + return true; + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_handleLocalization(QByteArray& bytes) +{ + QString errorMsg; + int errorLine; + QDomDocument doc; + if(!doc.setContent(bytes, false, &errorMsg, &errorLine)) { + qCritical() << "Unable to parse camera definition file on line:" << errorLine; + qCritical() << errorMsg; + return false; + } + //-- Find out where we are + QLocale locale = QLocale::system(); +#if defined (Q_OS_MAC) + locale = QLocale(locale.name()); +#endif + QString localeName = locale.name().toLower().replace("-", "_"); + qCDebug(CameraControlLog) << "Current locale:" << localeName; + if(localeName == "en_us") { + // Nothing to do + return true; + } + QDomNodeList locRoot = doc.elementsByTagName(kLocalization); + if(!locRoot.size()) { + // Nothing to do + return true; + } + //-- Iterate locales + QDomNode node = locRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList locales = elem.elementsByTagName(kLocale); + for(int i = 0; i < locales.size(); i++) { + QDomNode locale = locales.item(i); + QString name; + if(!read_attribute(locale, kName, name)) { + qWarning() << "Localization entry is missing its name attribute"; + continue; + } + // If we found a direct match, deal with it now + if(localeName == name.toLower().replace("-", "_")) { + return _replaceLocaleStrings(locale, bytes); + } + } + //-- No direct match. Pick first matching language (if any) + localeName = localeName.left(3); + for(int i = 0; i < locales.size(); i++) { + QDomNode locale = locales.item(i); + QString name; + read_attribute(locale, kName, name); + if(name.toLower().startsWith(localeName)) { + return _replaceLocaleStrings(locale, bytes); + } + } + //-- Could not find a language to use + qWarning() << "No match for" << QLocale::system().name() << "in camera definition file"; + //-- Just use default, en_US + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_replaceLocaleStrings(const QDomNode node, QByteArray& bytes) +{ + QDomElement stringElem = node.toElement(); + QDomNodeList strings = stringElem.elementsByTagName(kStrings); + for(int i = 0; i < strings.size(); i++) { + QDomNode stringNode = strings.item(i); + QString original; + QString translated; + if(read_attribute(stringNode, kOriginal, original)) { + if(read_attribute(stringNode, kTranslated, translated)) { + QString o; o = "\"" + original + "\""; + QString t; t = "\"" + translated + "\""; + bytes.replace(o.toUtf8(), t.toUtf8()); + o = ">" + original + "<"; + t = ">" + translated + "<"; + bytes.replace(o.toUtf8(), t.toUtf8()); + } + } + } + return true; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestAllParameters() +{ + //-- Reset receive list + for(const QString& paramName: _paramIO.keys()) { + if(_paramIO[paramName]) { + _paramIO[paramName]->setParamRequest(); + } else { + qCritical() << "QGCParamIO is NULL" << paramName; + } + } + WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); + if (!weakLink.expired()) { + SharedLinkInterfacePtr sharedLink = weakLink.lock(); + + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_message_t msg; + mavlink_msg_param_ext_request_list_pack_chan( + static_cast(mavlink->getSystemId()), + static_cast(mavlink->getComponentId()), + sharedLink->mavlinkChannel(), + &msg, + static_cast(_vehicle->id()), + static_cast(compID())); + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } + qCDebug(CameraControlVerboseLog) << "Request all parameters"; +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::_getParamName(const char* param_id) +{ + QByteArray bytes(param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + QString parameterName(bytes); + return parameterName; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleParamAck(const mavlink_param_ext_ack_t& ack) +{ + QString paramName = _getParamName(ack.param_id); + if(!_paramIO.contains(paramName)) { + qCWarning(CameraControlLog) << "Received PARAM_EXT_ACK for unknown param:" << paramName; + return; + } + if(_paramIO[paramName]) { + _paramIO[paramName]->handleParamAck(ack); + } else { + qCritical() << "QGCParamIO is NULL" << paramName; + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleParamValue(const mavlink_param_ext_value_t& value) +{ + QString paramName = _getParamName(value.param_id); + if(!_paramIO.contains(paramName)) { + qCWarning(CameraControlLog) << "Received PARAM_EXT_VALUE for unknown param:" << paramName; + return; + } + if(_paramIO[paramName]) { + _paramIO[paramName]->handleParamValue(value); + } else { + qCritical() << "QGCParamIO is NULL" << paramName; + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_updateActiveList() +{ + //-- Clear out excluded parameters based on exclusion rules + QStringList exclusionList; + for(QGCCameraOptionExclusion* param: _valueExclusions) { + Fact* pFact = getFact(param->param); + if(pFact) { + QString option = pFact->rawValueString(); + if(param->value == option) { + exclusionList << param->exclusions; + } + } + } + QStringList active; + for(QString key: _settings) { + if(!exclusionList.contains(key)) { + active.append(key); + } + } + if(active != _activeSettings) { + qCDebug(CameraControlVerboseLog) << "Excluding" << exclusionList; + _activeSettings = active; + emit activeSettingsChanged(); + //-- Force validity of "Facts" based on active set + if(_paramComplete) { + emit parametersReady(); + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_processConditionTest(const QString conditionTest) +{ + enum { + TEST_NONE, + TEST_EQUAL, + TEST_NOT_EQUAL, + TEST_GREATER, + TEST_SMALLER + }; + qCDebug(CameraControlVerboseLog) << "_processConditionTest(" << conditionTest << ")"; + int op = TEST_NONE; + QStringList test; + + auto split = [&conditionTest](const QString& sep ) { + return conditionTest.split(sep, Qt::SkipEmptyParts); + }; + + if(conditionTest.contains("!=")) { + test = split("!="); + op = TEST_NOT_EQUAL; + } else if(conditionTest.contains("=")) { + test = split("="); + op = TEST_EQUAL; + } else if(conditionTest.contains(">")) { + test = split(">"); + op = TEST_GREATER; + } else if(conditionTest.contains("<")) { + test = split("<"); + op = TEST_SMALLER; + } + if(test.size() == 2) { + Fact* pFact = getFact(test[0]); + if(pFact) { + switch(op) { + case TEST_EQUAL: + return pFact->rawValueString() == test[1]; + case TEST_NOT_EQUAL: + return pFact->rawValueString() != test[1]; + case TEST_GREATER: + return pFact->rawValueString() > test[1]; + case TEST_SMALLER: + return pFact->rawValueString() < test[1]; + case TEST_NONE: + break; + } + } else { + qWarning() << "Invalid condition parameter:" << test[0] << "in" << conditionTest; + return false; + } + } + qWarning() << "Invalid condition" << conditionTest; + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_processCondition(const QString condition) +{ + qCDebug(CameraControlVerboseLog) << "_processCondition(" << condition << ")"; + bool result = true; + bool andOp = true; + if(!condition.isEmpty()) { + QStringList scond = condition.split(" ", Qt::SkipEmptyParts); + while(scond.size()) { + QString test = scond.first(); + scond.removeFirst(); + if(andOp) { + result = result && _processConditionTest(test); + } else { + result = result || _processConditionTest(test); + } + if(!scond.size()) { + return result; + } + andOp = scond.first().toUpper() == "AND"; + scond.removeFirst(); + } + } + return result; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_updateRanges(Fact* pFact) +{ + QMap rangesSet; + QMap rangesReset; + QStringList changedList; + QStringList resetList; + QStringList updates; + //-- Iterate range sets looking for limited ranges + for(QGCCameraOptionRange* pRange: _optionRanges) { + //-- If this fact or one of its conditions is part of this range set + if(!changedList.contains(pRange->targetParam) && (pRange->param == pFact->name() || pRange->condition.contains(pFact->name()))) { + Fact* pRFact = getFact(pRange->param); //-- This parameter + Fact* pTFact = getFact(pRange->targetParam); //-- The target parameter (the one its range is to change) + if(pRFact && pTFact) { + //qCDebug(CameraControlVerboseLog) << "Check new set of options for" << pTFact->name(); + QString option = pRFact->rawValueString(); //-- This parameter value + //-- If this value (and condition) triggers a change in the target range + //qCDebug(CameraControlVerboseLog) << "Range value:" << pRange->value << "Current value:" << option << "Condition:" << pRange->condition; + if(pRange->value == option && _processCondition(pRange->condition)) { + if(pTFact->enumStrings() != pRange->optNames) { + //-- Set limited range set + rangesSet[pTFact] = pRange; + } + changedList << pRange->targetParam; + } + } + } + } + //-- Iterate range sets again looking for resets + for(QGCCameraOptionRange* pRange: _optionRanges) { + if(!changedList.contains(pRange->targetParam) && (pRange->param == pFact->name() || pRange->condition.contains(pFact->name()))) { + Fact* pTFact = getFact(pRange->targetParam); //-- The target parameter (the one its range is to change) + if(!resetList.contains(pRange->targetParam)) { + if(pTFact->enumStrings() != _originalOptNames[pRange->targetParam]) { + //-- Restore full option set + rangesReset[pTFact] = pRange->targetParam; + } + resetList << pRange->targetParam; + } + } + } + //-- Update limited range set + for (Fact* f: rangesSet.keys()) { + f->setEnumInfo(rangesSet[f]->optNames, rangesSet[f]->optVariants); + if(!updates.contains(f->name())) { + _paramIO[f->name()]->optNames = rangesSet[f]->optNames; + _paramIO[f->name()]->optVariants = rangesSet[f]->optVariants; + emit f->enumsChanged(); + qCDebug(CameraControlVerboseLog) << "Limited set of options for:" << f->name() << rangesSet[f]->optNames;; + updates << f->name(); + } + } + //-- Restore full range set + for (Fact* f: rangesReset.keys()) { + f->setEnumInfo(_originalOptNames[rangesReset[f]], _originalOptValues[rangesReset[f]]); + if(!updates.contains(f->name())) { + _paramIO[f->name()]->optNames = _originalOptNames[rangesReset[f]]; + _paramIO[f->name()]->optVariants = _originalOptValues[rangesReset[f]]; + emit f->enumsChanged(); + qCDebug(CameraControlVerboseLog) << "Restore full set of options for:" << f->name() << _originalOptNames[f->name()]; + updates << f->name(); + } + } + //-- Parameter update requests + if(_requestUpdates.contains(pFact->name())) { + for(const QString& param: _requestUpdates[pFact->name()]) { + if(!_updatesToRequest.contains(param)) { + _updatesToRequest << param; + } + } + } + if(_updatesToRequest.size()) { + QTimer::singleShot(500, this, &QGCCameraControl::_requestParamUpdates); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestParamUpdates() +{ + for(const QString& param: _updatesToRequest) { + _paramIO[param]->paramRequest(); + } + _updatesToRequest.clear(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestCameraSettings() +{ + qCDebug(CameraControlLog) << "_requestCameraSettings()"; + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_CAMERA_SETTINGS, // command id + false, // showError + 1); // Do Request + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestStorageInfo() +{ + qCDebug(CameraControlLog) << "_requestStorageInfo()"; + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_STORAGE_INFORMATION, // command id + false, // showError + 0, // Storage ID (0 for all, 1 for first, 2 for second, etc.) + 1); // Do Request + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleSettings(const mavlink_camera_settings_t& settings) +{ + qCDebug(CameraControlLog) << "handleSettings() Mode:" << settings.mode_id; + _setCameraMode(static_cast(settings.mode_id)); + qreal z = static_cast(settings.zoomLevel); + qreal f = static_cast(settings.focusLevel); + if(std::isfinite(z) && z != _zoomLevel) { + _zoomLevel = z; + emit zoomLevelChanged(); + } + if(std::isfinite(f) && f != _focusLevel) { + _focusLevel = f; + emit focusLevelChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleStorageInfo(const mavlink_storage_information_t& st) +{ + qCDebug(CameraControlLog) << "handleStorageInfo:" << st.available_capacity << st.status << st.storage_count << st.storage_id << st.total_capacity << st.used_capacity; + if(st.status == STORAGE_STATUS_READY) { + uint32_t t = static_cast(st.total_capacity); + if(_storageTotal != t) { + _storageTotal = t; + emit storageTotalChanged(); + } + uint32_t a = static_cast(st.available_capacity); + if(_storageFree != a) { + _storageFree = a; + emit storageFreeChanged(); + } + } + if(_storageStatus != static_cast(st.status)) { + _storageStatus = static_cast(st.status); + emit storageStatusChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleBatteryStatus(const mavlink_battery_status_t& bs) +{ + qCDebug(CameraControlLog) << "handleBatteryStatus:" << bs.battery_remaining; + if(bs.battery_remaining >= 0 && _batteryRemaining != static_cast(bs.battery_remaining)) { + _batteryRemaining = static_cast(bs.battery_remaining); + emit batteryRemainingChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleCaptureStatus(const mavlink_camera_capture_status_t& cap) +{ + //-- This is a response to MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS + qCDebug(CameraControlLog) << "handleCaptureStatus:" << cap.available_capacity << cap.image_interval << cap.image_status << cap.recording_time_ms << cap.video_status; + //-- Disk Free Space + uint32_t a = static_cast(cap.available_capacity); + if(_storageFree != a) { + _storageFree = a; + emit storageFreeChanged(); + } + //-- Do we have recording time? + if(cap.recording_time_ms) { + // Resync our _recTime timer to the time info received from the camera component + _recordTime = cap.recording_time_ms; + _recTime = _recTime.addMSecs(_recTime.msecsTo(QTime::currentTime()) - static_cast(cap.recording_time_ms)); + emit recordTimeChanged(); + } + //-- Video/Image Capture Status + uint8_t vs = cap.video_status < static_cast(VIDEO_CAPTURE_STATUS_LAST) ? cap.video_status : static_cast(VIDEO_CAPTURE_STATUS_UNDEFINED); + uint8_t ps = cap.image_status < static_cast(PHOTO_CAPTURE_LAST) ? cap.image_status : static_cast(PHOTO_CAPTURE_STATUS_UNDEFINED); + _setVideoStatus(static_cast(vs)); + _setPhotoStatus(static_cast(ps)); + //-- Keep asking for it once in a while when recording + if(videoStatus() == VIDEO_CAPTURE_STATUS_RUNNING) { + _captureStatusTimer.start(5000); + //-- Same while (single) image capture is busy + } else if(photoStatus() != PHOTO_CAPTURE_IDLE && photoMode() == PHOTO_CAPTURE_SINGLE) { + _captureStatusTimer.start(1000); + } + //-- Time Lapse + if(photoStatus() == PHOTO_CAPTURE_INTERVAL_IDLE || photoStatus() == PHOTO_CAPTURE_INTERVAL_IN_PROGRESS) { + //-- Capture local image as well + if(qgcApp()->toolbox()->videoManager()) { + QString photoPath = qgcApp()->toolbox()->settingsManager()->appSettings()->savePath()->rawValue().toString() + QStringLiteral("/Photo"); + QDir().mkpath(photoPath); + photoPath += + "/" + QDateTime::currentDateTime().toString("yyyy-MM-dd_hh.mm.ss.zzz") + ".jpg"; + qgcApp()->toolbox()->videoManager()->grabImage(photoPath); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleVideoInfo(const mavlink_video_stream_information_t* vi) +{ + qCDebug(CameraControlLog) << "handleVideoInfo:" << vi->stream_id << vi->uri; + _expectedCount = vi->count; + if(!_findStream(vi->stream_id, false)) { + qCDebug(CameraControlLog) << "Create stream handler for stream ID:" << vi->stream_id; + QGCVideoStreamInfo* pStream = new QGCVideoStreamInfo(this, vi); + QQmlEngine::setObjectOwnership(pStream, QQmlEngine::CppOwnership); + _streams.append(pStream); + //-- Thermal is handled separately and not listed + if(!pStream->isThermal()) { + _streamLabels.append(pStream->name()); + emit streamsChanged(); + emit streamLabelsChanged(); + } else { + emit thermalStreamChanged(); + } + } + //-- Check for missing count + if(_streams.count() < _expectedCount) { + _streamInfoTimer.start(1000); + } else { + //-- Done + qCDebug(CameraControlLog) << "All stream handlers done"; + _streamInfoTimer.stop(); + emit autoStreamChanged(); + emit _vehicle->cameraManager()->streamChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleVideoStatus(const mavlink_video_stream_status_t* vs) +{ + _streamStatusTimer.stop(); + qCDebug(CameraControlLog) << "handleVideoStatus:" << vs->stream_id; + QGCVideoStreamInfo* pInfo = _findStream(vs->stream_id); + if(pInfo) { + pInfo->update(vs); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setCurrentStream(int stream) +{ + if(stream != _currentStream && stream >= 0 && stream < _streamLabels.count()) { + if(_currentStream != stream) { + QGCVideoStreamInfo* pInfo = currentStreamInstance(); + if(pInfo) { + qCDebug(CameraControlLog) << "Stopping stream:" << pInfo->uri(); + //-- Stop current stream + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_STOP_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + } + _currentStream = stream; + pInfo = currentStreamInstance(); + if(pInfo) { + //-- Start new stream + qCDebug(CameraControlLog) << "Starting stream:" << pInfo->uri(); + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_START_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + //-- Update stream status + _requestStreamStatus(static_cast(pInfo->streamID())); + } + emit currentStreamChanged(); + emit _vehicle->cameraManager()->streamChanged(); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::stopStream() +{ + QGCVideoStreamInfo* pInfo = currentStreamInstance(); + if(pInfo) { + //-- Stop current stream + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_STOP_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::resumeStream() +{ + QGCVideoStreamInfo* pInfo = currentStreamInstance(); + if(pInfo) { + //-- Start new stream + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_START_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::autoStream() +{ + if(hasVideoStream()) { + return _streams.count() > 0; + } + return false; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::currentStreamInstance() +{ + if(_currentStream < _streamLabels.count() && _streamLabels.count()) { + QGCVideoStreamInfo* pStream = _findStream(_streamLabels[_currentStream]); + return pStream; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::thermalStreamInstance() +{ + //-- For now, it will return the first thermal listed (if any) + for(int i = 0; i < _streams.count(); i++) { + if(_streams[i]) { + QGCVideoStreamInfo* pStream = qobject_cast(_streams[i]); + if(pStream) { + if(pStream->isThermal()) { + return pStream; + } + } + } + } + return nullptr; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestStreamInfo(uint8_t streamID) +{ + qCDebug(CameraControlLog) << "Requesting video stream info for:" << streamID; + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION, // Command id + false, // ShowError + streamID); // Stream ID +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestStreamStatus(uint8_t streamID) +{ + qCDebug(CameraControlLog) << "Requesting video stream status for:" << streamID; + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_VIDEO_STREAM_STATUS, // Command id + false, // ShowError + streamID); // Stream ID + _streamStatusTimer.start(1000); // Wait up to a second for it +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::_findStream(uint8_t id, bool report) +{ + for(int i = 0; i < _streams.count(); i++) { + if(_streams[i]) { + QGCVideoStreamInfo* pStream = qobject_cast(_streams[i]); + if(pStream) { + if(pStream->streamID() == id) { + return pStream; + } + } else { + qCritical() << "Null QGCVideoStreamInfo instance"; + } + } + } + if(report) { + qWarning() << "Stream id not found:" << id; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::_findStream(const QString name) +{ + for(int i = 0; i < _streams.count(); i++) { + if(_streams[i]) { + QGCVideoStreamInfo* pStream = qobject_cast(_streams[i]); + if(pStream) { + if(pStream->name() == name) { + return pStream; + } + } + } + } + return nullptr; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_streamTimeout() +{ + _requestCount++; + int count = _expectedCount * 3; + if(_requestCount > count) { + qCWarning(CameraControlLog) << "Giving up requesting video stream info"; + _streamInfoTimer.stop(); + //-- If we have at least one stream, work with what we have. + if(_streams.count()) { + emit autoStreamChanged(); + emit _vehicle->cameraManager()->streamChanged(); + } + return; + } + for(uint8_t i = 0; i < _expectedCount; i++) { + //-- Stream ID starts at 1 + if(!_findStream(i+1, false)) { + _requestStreamInfo(i+1); + return; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_streamStatusTimeout() +{ + QGCVideoStreamInfo* pStream = currentStreamInstance(); + if(pStream) { + _requestStreamStatus(static_cast(pStream->streamID())); + } +} + +//----------------------------------------------------------------------------- +QStringList +QGCCameraControl::_loadExclusions(QDomNode option) +{ + QStringList exclusionList; + QDomElement optionElem = option.toElement(); + QDomNodeList excRoot = optionElem.elementsByTagName(kExclusions); + if(excRoot.size()) { + //-- Iterate exclusions + QDomNode node = excRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList exclusions = elem.elementsByTagName(kExclusion); + for(int i = 0; i < exclusions.size(); i++) { + QString exclude = exclusions.item(i).toElement().text(); + if(!exclude.isEmpty()) { + exclusionList << exclude; + } + } + } + return exclusionList; +} + +//----------------------------------------------------------------------------- +QStringList +QGCCameraControl::_loadUpdates(QDomNode option) +{ + QStringList updateList; + QDomElement optionElem = option.toElement(); + QDomNodeList updateRoot = optionElem.elementsByTagName(kUpdates); + if(updateRoot.size()) { + //-- Iterate updates + QDomNode node = updateRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList updates = elem.elementsByTagName(kUpdate); + for(int i = 0; i < updates.size(); i++) { + QString update = updates.item(i).toElement().text(); + if(!update.isEmpty()) { + updateList << update; + } + } + } + return updateList; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadRanges(QDomNode option, const QString factName, QString paramValue) +{ + QDomElement optionElem = option.toElement(); + QDomNodeList rangeRoot = optionElem.elementsByTagName(kParameterranges); + if(rangeRoot.size()) { + QDomNode node = rangeRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList parameterRanges = elem.elementsByTagName(kParameterrange); + //-- Iterate parameter ranges + for(int i = 0; i < parameterRanges.size(); i++) { + QString param; + QString condition; + QDomNode paramRange = parameterRanges.item(i); + if(!read_attribute(paramRange, kParameter, param)) { + qCritical() << QString("Malformed option range for parameter %1").arg(factName); + return false; + } + read_attribute(paramRange, kCondition, condition); + QDomElement pelem = paramRange.toElement(); + QDomNodeList rangeOptions = pelem.elementsByTagName(kRoption); + QStringList optNames; + QStringList optValues; + //-- Iterate options + for(int i = 0; i < rangeOptions.size(); i++) { + QString optName; + QString optValue; + QDomNode roption = rangeOptions.item(i); + if(!read_attribute(roption, kName, optName)) { + qCritical() << QString("Malformed roption for parameter %1").arg(factName); + return false; + } + if(!read_attribute(roption, kValue, optValue)) { + qCritical() << QString("Malformed rvalue for parameter %1").arg(factName); + return false; + } + optNames << optName; + optValues << optValue; + } + if(optNames.size()) { + QGCCameraOptionRange* pRange = new QGCCameraOptionRange(this, factName, paramValue, param, condition, optNames, optValues); + _optionRanges.append(pRange); + qCDebug(CameraControlVerboseLog) << "New range limit:" << factName << paramValue << param << condition << optNames << optValues; + } + } + } + return true; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_processRanges() +{ + //-- After all parameter are loaded, process parameter ranges + for(QGCCameraOptionRange* pRange: _optionRanges) { + Fact* pRFact = getFact(pRange->targetParam); + if(pRFact) { + for(int i = 0; i < pRange->optNames.size(); i++) { + QVariant optVariant; + QString errorString; + if (!pRFact->metaData()->convertAndValidateRaw(pRange->optValues[i], false, optVariant, errorString)) { + qWarning() << "Invalid roption value, name:" << pRange->targetParam + << " type:" << pRFact->metaData()->type() + << " value:" << pRange->optValues[i] + << " error:" << errorString; + } else { + pRange->optVariants << optVariant; + } + } + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadNameValue(QDomNode option, const QString factName, FactMetaData* metaData, QString& optName, QString& optValue, QVariant& optVariant) +{ + if(!read_attribute(option, kName, optName)) { + qCritical() << QString("Malformed option for parameter %1").arg(factName); + return false; + } + if(!read_attribute(option, kValue, optValue)) { + qCritical() << QString("Malformed value for parameter %1").arg(factName); + return false; + } + QString errorString; + if (!metaData->convertAndValidateRaw(optValue, false, optVariant, errorString)) { + qWarning() << "Invalid option value, name:" << factName + << " type:" << metaData->type() + << " value:" << optValue + << " error:" << errorString; + } + return true; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_handleDefinitionFile(const QString &url) +{ + //-- First check and see if we have it cached + QFile xmlFile(_cacheFile); + + QString ftpPrefix(QStringLiteral("%1://").arg(FTPManager::mavlinkFTPScheme)); + if (!xmlFile.exists() && url.startsWith(ftpPrefix, Qt::CaseInsensitive)) { + qCDebug(CameraControlLog) << "No camera definition file cached, attempt ftp download"; + int ver = static_cast(_info.cam_definition_version); + QString ext = ""; + if (url.endsWith(".lzma", Qt::CaseInsensitive)) { ext = ".lzma"; } + if (url.endsWith(".xz", Qt::CaseInsensitive)) { ext = ".xz"; } + QString fileName = QString::asprintf("%s_%s_%03d.xml%s", + _vendor.toStdString().c_str(), + _modelName.toStdString().c_str(), + ver, + ext.toStdString().c_str()); + connect(_vehicle->ftpManager(), &FTPManager::downloadComplete, this, &QGCCameraControl::_ftpDownloadComplete); + _vehicle->ftpManager()->download(_compID, url, + qgcApp()->toolbox()->settingsManager()->appSettings()->parameterSavePath().toStdString().c_str(), + fileName); + return; + } + + if (!xmlFile.exists()) { + qCDebug(CameraControlLog) << "No camera definition file cached, attempt http download"; + _httpRequest(url); + return; + } + if (!xmlFile.open(QIODevice::ReadOnly)) { + qWarning() << "Could not read cached camera definition file:" << _cacheFile; + _httpRequest(url); + return; + } + QByteArray bytes = xmlFile.readAll(); + QDomDocument doc; + if(!doc.setContent(bytes, false)) { + qWarning() << "Could not parse cached camera definition file:" << _cacheFile; + _httpRequest(url); + return; + } + //-- We have it + qCDebug(CameraControlLog) << "Using cached camera definition file:" << _cacheFile; + _cached = true; + emit dataReady(bytes); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_httpRequest(const QString &url) +{ + qCDebug(CameraControlLog) << "Request camera definition:" << url; + if(!_netManager) { + _netManager = new QNetworkAccessManager(this); + } + QNetworkProxy savedProxy = _netManager->proxy(); + QNetworkProxy tempProxy; + tempProxy.setType(QNetworkProxy::DefaultProxy); + _netManager->setProxy(tempProxy); + QNetworkRequest request(url); + request.setAttribute(QNetworkRequest::RedirectPolicyAttribute, true); + QSslConfiguration conf = request.sslConfiguration(); + conf.setPeerVerifyMode(QSslSocket::VerifyNone); + request.setSslConfiguration(conf); + QNetworkReply* reply = _netManager->get(request); + connect(reply, &QNetworkReply::finished, this, &QGCCameraControl::_downloadFinished); + _netManager->setProxy(savedProxy); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_downloadFinished() +{ + QNetworkReply* reply = qobject_cast(sender()); + if(!reply) { + return; + } + int err = reply->error(); + int http_code = reply->attribute(QNetworkRequest::HttpStatusCodeAttribute).toInt(); + QByteArray data = reply->readAll(); + if(err == QNetworkReply::NoError && http_code == 200) { + data.append("\n"); + } else { + data.clear(); + qWarning() << QString("Camera Definition (%1) download error: %2 status: %3").arg( + reply->url().toDisplayString(), + reply->errorString(), + reply->attribute(QNetworkRequest::HttpStatusCodeAttribute).toString() + ); + } + emit dataReady(data); + //reply->deleteLater(); +} + +void QGCCameraControl::_ftpDownloadComplete(const QString& fileName, const QString& errorMsg) +{ + qCDebug(CameraControlLog) << "FTP Download completed: " << fileName << ", " << errorMsg; + + disconnect(_vehicle->ftpManager(), &FTPManager::downloadComplete, this, &QGCCameraControl::_ftpDownloadComplete); + + QString outputFileName = fileName; + + if (fileName.endsWith(".lzma", Qt::CaseInsensitive) || fileName.endsWith(".xz", Qt::CaseInsensitive)) { + outputFileName = fileName.left(fileName.lastIndexOf(".")); + if (QGCLZMA::inflateLZMAFile(fileName, outputFileName)) { + QFile(fileName).remove(); + } else { + qCWarning(CameraControlLog) << "Inflate of compressed xml failed" << fileName; + outputFileName.clear(); + } + } + + QFile xmlFile(outputFileName); + + if (!xmlFile.exists()) { + qCDebug(CameraControlLog) << "No camera definition file present after ftp download completed"; + return; + } + if (!xmlFile.open(QIODevice::ReadOnly)) { + qWarning() << "Could not read downloaded camera definition file: " << fileName; + return; + } + + _cached = true; + QByteArray bytes = xmlFile.readAll(); + emit dataReady(bytes); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_dataReady(QByteArray data) +{ + if(data.size()) { + qCDebug(CameraControlLog) << "Parsing camera definition"; + _loadCameraDefinitionFile(data); + } else { + qCDebug(CameraControlLog) << "No camera definition"; + } + _initWhenReady(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_paramDone() +{ + for(const QString& param: _paramIO.keys()) { + if(!_paramIO[param]->paramDone()) { + return; + } + } + //-- All parameters loaded (or timed out) + _paramComplete = true; + emit parametersReady(); + //-- Check for video streaming + _checkForVideoStreams(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_checkForVideoStreams() +{ + if(_info.flags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM) { + //-- Skip it if using Taisync as it has its own video settings + if(!qgcApp()->toolbox()->videoManager()->isTaisync()) { + connect(&_streamInfoTimer, &QTimer::timeout, this, &QGCCameraControl::_streamTimeout); + _streamInfoTimer.setSingleShot(false); + connect(&_streamStatusTimer, &QTimer::timeout, this, &QGCCameraControl::_streamStatusTimeout); + _streamStatusTimer.setSingleShot(true); + //-- Request all streams + _requestStreamInfo(0); + _streamInfoTimer.start(2000); + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::incomingParameter(Fact* pFact, QVariant& newValue) +{ + Q_UNUSED(pFact); + Q_UNUSED(newValue); + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::validateParameter(Fact* pFact, QVariant& newValue) +{ + Q_UNUSED(pFact); + Q_UNUSED(newValue); + return true; +} + +//----------------------------------------------------------------------------- +QStringList +QGCCameraControl::activeSettings() +{ + qCDebug(CameraControlLog) << "Active:" << _activeSettings; + return _activeSettings; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::exposureMode() +{ + return (_paramComplete && _activeSettings.contains(kCAM_EXPMODE)) ? getFact(kCAM_EXPMODE) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::ev() +{ + return (_paramComplete && _activeSettings.contains(kCAM_EV)) ? getFact(kCAM_EV) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::iso() +{ + return (_paramComplete && _activeSettings.contains(kCAM_ISO)) ? getFact(kCAM_ISO) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::shutterSpeed() +{ + return (_paramComplete && _activeSettings.contains(kCAM_SHUTTERSPD)) ? getFact(kCAM_SHUTTERSPD) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::aperture() +{ + return (_paramComplete && _activeSettings.contains(kCAM_APERTURE)) ? getFact(kCAM_APERTURE) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::wb() +{ + return (_paramComplete && _activeSettings.contains(kCAM_WBMODE)) ? getFact(kCAM_WBMODE) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::mode() +{ + return _paramComplete && factExists(kCAM_MODE) ? getFact(kCAM_MODE) : nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo::QGCVideoStreamInfo(QObject* parent, const mavlink_video_stream_information_t *si) + : QObject(parent) +{ + memcpy(&_streamInfo, si, sizeof(mavlink_video_stream_information_t)); +} + +//----------------------------------------------------------------------------- +qreal +QGCVideoStreamInfo::aspectRatio() const +{ + qreal ar = 1.0; + if(_streamInfo.resolution_h && _streamInfo.resolution_v) { + ar = static_cast(_streamInfo.resolution_h) / static_cast(_streamInfo.resolution_v); + } + return ar; +} + +//----------------------------------------------------------------------------- +bool +QGCVideoStreamInfo::update(const mavlink_video_stream_status_t* vs) +{ + bool changed = false; + if(_streamInfo.hfov != vs->hfov) { + changed = true; + _streamInfo.hfov = vs->hfov; + } + if(_streamInfo.flags != vs->flags) { + changed = true; + _streamInfo.flags = vs->flags; + } + if(_streamInfo.bitrate != vs->bitrate) { + changed = true; + _streamInfo.bitrate = vs->bitrate; + } + if(_streamInfo.rotation != vs->rotation) { + changed = true; + _streamInfo.rotation = vs->rotation; + } + if(_streamInfo.framerate != vs->framerate) { + changed = true; + _streamInfo.framerate = vs->framerate; + } + if(_streamInfo.resolution_h != vs->resolution_h) { + changed = true; + _streamInfo.resolution_h = vs->resolution_h; + } + if(_streamInfo.resolution_v != vs->resolution_v) { + changed = true; + _streamInfo.resolution_v = vs->resolution_v; + } + if(changed) { + emit infoChanged(); + } + return changed; +} diff --git a/src/Camera/QGCCameraControl.h b/src/Camera/QGCCameraControl.h new file mode 100644 index 0000000..a34194e --- /dev/null +++ b/src/Camera/QGCCameraControl.h @@ -0,0 +1,422 @@ + + +#pragma once + +#include "QGCApplication.h" +#include + +class QDomNode; +class QDomNodeList; +class QGCCameraParamIO; + +Q_DECLARE_LOGGING_CATEGORY(CameraControlLog) +Q_DECLARE_LOGGING_CATEGORY(CameraControlVerboseLog) + +//----------------------------------------------------------------------------- +/// Video Stream Info +/// Encapsulates the contents of a [VIDEO_STREAM_INFORMATION](https://mavlink.io/en/messages/common.html#VIDEO_STREAM_INFORMATION) message +class QGCVideoStreamInfo : public QObject +{ + Q_OBJECT +public: + QGCVideoStreamInfo(QObject* parent, const mavlink_video_stream_information_t* si); + + Q_PROPERTY(QString uri READ uri NOTIFY infoChanged) + Q_PROPERTY(QString name READ name NOTIFY infoChanged) + Q_PROPERTY(int streamID READ streamID NOTIFY infoChanged) + Q_PROPERTY(int type READ type NOTIFY infoChanged) + Q_PROPERTY(qreal aspectRatio READ aspectRatio NOTIFY infoChanged) + Q_PROPERTY(qreal hfov READ hfov NOTIFY infoChanged) + Q_PROPERTY(bool isThermal READ isThermal NOTIFY infoChanged) + + QString uri () { return QString(_streamInfo.uri); } + QString name () { return QString(_streamInfo.name); } + qreal aspectRatio () const; + qreal hfov () const{ return _streamInfo.hfov; } + int type () const{ return _streamInfo.type; } + int streamID () const{ return _streamInfo.stream_id; } + bool isThermal () const{ return _streamInfo.flags & VIDEO_STREAM_STATUS_FLAGS_THERMAL; } + + bool update (const mavlink_video_stream_status_t* vs); + +signals: + void infoChanged (); + +private: + mavlink_video_stream_information_t _streamInfo; +}; + +//----------------------------------------------------------------------------- +/// Camera option exclusions +class QGCCameraOptionExclusion : public QObject +{ +public: + QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_); + QString param; + QString value; + QStringList exclusions; +}; + +//----------------------------------------------------------------------------- +/// Camera option ranges +class QGCCameraOptionRange : public QObject +{ +public: + QGCCameraOptionRange(QObject* parent, QString param_, QString value_, QString targetParam_, QString condition_, QStringList optNames_, QStringList optValues_); + QString param; + QString value; + QString targetParam; + QString condition; + QStringList optNames; + QStringList optValues; + QVariantList optVariants; +}; + +//----------------------------------------------------------------------------- +/// MAVLink Camera API controller +class QGCCameraControl : public FactGroup +{ + Q_OBJECT + friend class QGCCameraParamIO; +public: + QGCCameraControl(const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = nullptr); + virtual ~QGCCameraControl(); + + //-- cam_mode + enum CameraMode { + CAM_MODE_UNDEFINED = -1, + CAM_MODE_PHOTO = 0, + CAM_MODE_VIDEO = 1, + CAM_MODE_SURVEY = 2, + }; + + //-- Video Capture Status + enum VideoStatus { + VIDEO_CAPTURE_STATUS_STOPPED = 0, + VIDEO_CAPTURE_STATUS_RUNNING, + VIDEO_CAPTURE_STATUS_LAST, + VIDEO_CAPTURE_STATUS_UNDEFINED = 255 + }; + + //-- Photo Capture Status + enum PhotoStatus { + PHOTO_CAPTURE_IDLE = 0, + PHOTO_CAPTURE_IN_PROGRESS, + PHOTO_CAPTURE_INTERVAL_IDLE, + PHOTO_CAPTURE_INTERVAL_IN_PROGRESS, + PHOTO_CAPTURE_LAST, + PHOTO_CAPTURE_STATUS_UNDEFINED = 255 + }; + + //-- Photo Capture Modes + enum PhotoMode { + PHOTO_CAPTURE_SINGLE = 0, + PHOTO_CAPTURE_TIMELAPSE, + }; + + //-- Storage Status + enum StorageStatus { + STORAGE_EMPTY = STORAGE_STATUS_EMPTY, + STORAGE_UNFORMATTED = STORAGE_STATUS_UNFORMATTED, + STORAGE_READY = STORAGE_STATUS_READY, + STORAGE_NOT_SUPPORTED = STORAGE_STATUS_NOT_SUPPORTED + }; + + enum ThermalViewMode { + THERMAL_OFF = 0, + THERMAL_BLEND, + THERMAL_FULL, + THERMAL_PIP, + }; + + Q_ENUM(CameraMode) + Q_ENUM(VideoStatus) + Q_ENUM(PhotoStatus) + Q_ENUM(PhotoMode) + Q_ENUM(StorageStatus) + Q_ENUM(ThermalViewMode) + + Q_PROPERTY(int version READ version NOTIFY infoChanged) + Q_PROPERTY(QString modelName READ modelName NOTIFY infoChanged) + Q_PROPERTY(QString vendor READ vendor NOTIFY infoChanged) + Q_PROPERTY(QString firmwareVersion READ firmwareVersion NOTIFY infoChanged) + Q_PROPERTY(qreal focalLength READ focalLength NOTIFY infoChanged) + Q_PROPERTY(QSizeF sensorSize READ sensorSize NOTIFY infoChanged) + Q_PROPERTY(QSize resolution READ resolution NOTIFY infoChanged) + Q_PROPERTY(bool capturesVideo READ capturesVideo NOTIFY infoChanged) + Q_PROPERTY(bool capturesPhotos READ capturesPhotos NOTIFY infoChanged) + Q_PROPERTY(bool hasModes READ hasModes NOTIFY infoChanged) + Q_PROPERTY(bool hasZoom READ hasZoom NOTIFY infoChanged) + Q_PROPERTY(bool hasFocus READ hasFocus NOTIFY infoChanged) + Q_PROPERTY(bool hasVideoStream READ hasVideoStream NOTIFY infoChanged) + Q_PROPERTY(bool photosInVideoMode READ photosInVideoMode NOTIFY infoChanged) + Q_PROPERTY(bool videoInPhotoMode READ videoInPhotoMode NOTIFY infoChanged) + Q_PROPERTY(bool isBasic READ isBasic NOTIFY infoChanged) + Q_PROPERTY(quint32 storageFree READ storageFree NOTIFY storageFreeChanged) + Q_PROPERTY(QString storageFreeStr READ storageFreeStr NOTIFY storageFreeChanged) + Q_PROPERTY(quint32 storageTotal READ storageTotal NOTIFY storageTotalChanged) + Q_PROPERTY(int batteryRemaining READ batteryRemaining NOTIFY batteryRemainingChanged) + Q_PROPERTY(QString batteryRemainingStr READ batteryRemainingStr NOTIFY batteryRemainingChanged) + Q_PROPERTY(bool paramComplete READ paramComplete NOTIFY parametersReady) + + Q_PROPERTY(qreal zoomLevel READ zoomLevel WRITE setZoomLevel NOTIFY zoomLevelChanged) + Q_PROPERTY(qreal focusLevel READ focusLevel WRITE setFocusLevel NOTIFY focusLevelChanged) + + Q_PROPERTY(Fact* exposureMode READ exposureMode NOTIFY parametersReady) + Q_PROPERTY(Fact* ev READ ev NOTIFY parametersReady) + Q_PROPERTY(Fact* iso READ iso NOTIFY parametersReady) + Q_PROPERTY(Fact* shutterSpeed READ shutterSpeed NOTIFY parametersReady) + Q_PROPERTY(Fact* aperture READ aperture NOTIFY parametersReady) + Q_PROPERTY(Fact* wb READ wb NOTIFY parametersReady) + Q_PROPERTY(Fact* mode READ mode NOTIFY parametersReady) + + Q_PROPERTY(QStringList activeSettings READ activeSettings NOTIFY activeSettingsChanged) + Q_PROPERTY(VideoStatus videoStatus READ videoStatus NOTIFY videoStatusChanged) + Q_PROPERTY(PhotoStatus photoStatus READ photoStatus NOTIFY photoStatusChanged) + Q_PROPERTY(CameraMode cameraMode READ cameraMode WRITE setCameraMode NOTIFY cameraModeChanged) + Q_PROPERTY(StorageStatus storageStatus READ storageStatus NOTIFY storageStatusChanged) + Q_PROPERTY(qreal photoLapse READ photoLapse WRITE setPhotoLapse NOTIFY photoLapseChanged) + Q_PROPERTY(int photoLapseCount READ photoLapseCount WRITE setPhotoLapseCount NOTIFY photoLapseCountChanged) + Q_PROPERTY(PhotoMode photoMode READ photoMode WRITE setPhotoMode NOTIFY photoModeChanged) + Q_PROPERTY(int currentStream READ currentStream WRITE setCurrentStream NOTIFY currentStreamChanged) + Q_PROPERTY(bool autoStream READ autoStream NOTIFY autoStreamChanged) + Q_PROPERTY(QmlObjectListModel* streams READ streams NOTIFY streamsChanged) + Q_PROPERTY(QGCVideoStreamInfo* currentStreamInstance READ currentStreamInstance NOTIFY currentStreamChanged) + Q_PROPERTY(QGCVideoStreamInfo* thermalStreamInstance READ thermalStreamInstance NOTIFY thermalStreamChanged) + Q_PROPERTY(quint32 recordTime READ recordTime NOTIFY recordTimeChanged) + Q_PROPERTY(QString recordTimeStr READ recordTimeStr NOTIFY recordTimeChanged) + Q_PROPERTY(QStringList streamLabels READ streamLabels NOTIFY streamLabelsChanged) + Q_PROPERTY(ThermalViewMode thermalMode READ thermalMode WRITE setThermalMode NOTIFY thermalModeChanged) + Q_PROPERTY(double thermalOpacity READ thermalOpacity WRITE setThermalOpacity NOTIFY thermalOpacityChanged) + + Q_INVOKABLE virtual void setVideoMode (); + Q_INVOKABLE virtual void setPhotoMode (); + Q_INVOKABLE virtual void toggleMode (); + Q_INVOKABLE virtual bool takePhoto (); + Q_INVOKABLE virtual bool stopTakePhoto (); + Q_INVOKABLE virtual bool startVideo (); + Q_INVOKABLE virtual bool stopVideo (); + Q_INVOKABLE virtual bool toggleVideo (); + Q_INVOKABLE virtual void resetSettings (); + Q_INVOKABLE virtual void formatCard (int id = 1); + Q_INVOKABLE virtual void stepZoom (int direction); + Q_INVOKABLE virtual void startZoom (int direction); + Q_INVOKABLE virtual void stopZoom (); + Q_INVOKABLE virtual void stopStream (); + Q_INVOKABLE virtual void resumeStream (); + + virtual int version () { return _version; } + virtual QString modelName () { return _modelName; } + virtual QString vendor () { return _vendor; } + virtual QString firmwareVersion (); + virtual qreal focalLength () { return static_cast(_info.focal_length); } + virtual QSizeF sensorSize () { return QSizeF(static_cast(_info.sensor_size_h), static_cast(_info.sensor_size_v)); } + virtual QSize resolution () { return QSize(_info.resolution_h, _info.resolution_v); } + virtual bool capturesVideo () { return _info.flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO; } + virtual bool capturesPhotos () { return _info.flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE; } + virtual bool hasModes () { return _info.flags & CAMERA_CAP_FLAGS_HAS_MODES; } + virtual bool hasZoom () { return _info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM; } + virtual bool hasFocus () { return _info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS; } + virtual bool hasVideoStream () { return _info.flags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM; } + virtual bool photosInVideoMode () { return _info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE; } + virtual bool videoInPhotoMode () { return _info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE; } + + virtual int compID () { return _compID; } + virtual bool isBasic () { return _settings.size() == 0; } + virtual VideoStatus videoStatus (); + virtual PhotoStatus photoStatus (); + virtual PhotoMode photoMode () { return _photoMode; } + virtual qreal photoLapse () { return _photoLapse; } + virtual int photoLapseCount () { return _photoLapseCount; } + virtual CameraMode cameraMode () { return _cameraMode; } + virtual StorageStatus storageStatus () { return _storageStatus; } + virtual QStringList activeSettings (); + virtual quint32 storageFree () { return _storageFree; } + virtual QString storageFreeStr (); + virtual quint32 storageTotal () { return _storageTotal; } + virtual int batteryRemaining () { return _batteryRemaining; } + virtual QString batteryRemainingStr (); + virtual bool paramComplete () { return _paramComplete; } + virtual qreal zoomLevel () { return _zoomLevel; } + virtual qreal focusLevel () { return _focusLevel; } + + virtual QmlObjectListModel* streams () { return &_streams; } + virtual QGCVideoStreamInfo* currentStreamInstance(); + virtual QGCVideoStreamInfo* thermalStreamInstance(); + virtual int currentStream () { return _currentStream; } + virtual void setCurrentStream (int stream); + virtual bool autoStream (); + virtual quint32 recordTime () { return _recordTime; } + virtual QString recordTimeStr (); + + virtual Fact* exposureMode (); + virtual Fact* ev (); + virtual Fact* iso (); + virtual Fact* shutterSpeed (); + virtual Fact* aperture (); + virtual Fact* wb (); + virtual Fact* mode (); + + /// Stream names to show the user (for selection) + virtual QStringList streamLabels () { return _streamLabels; } + + virtual ThermalViewMode thermalMode () { return _thermalMode; } + virtual void setThermalMode (ThermalViewMode mode); + virtual double thermalOpacity () { return _thermalOpacity; } + virtual void setThermalOpacity (double val); + + virtual void setZoomLevel (qreal level); + virtual void setFocusLevel (qreal level); + virtual void setCameraMode (CameraMode mode); + virtual void setPhotoMode (PhotoMode mode); + virtual void setPhotoLapse (qreal interval); + virtual void setPhotoLapseCount (int count); + + virtual void handleSettings (const mavlink_camera_settings_t& settings); + virtual void handleCaptureStatus (const mavlink_camera_capture_status_t& capStatus); + virtual void handleParamAck (const mavlink_param_ext_ack_t& ack); + virtual void handleParamValue (const mavlink_param_ext_value_t& value); + virtual void handleStorageInfo (const mavlink_storage_information_t& st); + virtual void handleBatteryStatus (const mavlink_battery_status_t& bs); + virtual void handleVideoInfo (const mavlink_video_stream_information_t *vi); + virtual void handleVideoStatus (const mavlink_video_stream_status_t *vs); + + /// Notify controller a parameter has changed + virtual void factChanged (Fact* pFact); + /// Allow controller to modify or invalidate incoming parameter + virtual bool incomingParameter (Fact* pFact, QVariant& newValue); + /// Allow controller to modify or invalidate parameter change + virtual bool validateParameter (Fact* pFact, QVariant& newValue); + + // Known Parameters + static const char* kCAM_EV; + static const char* kCAM_EXPMODE; + static const char* kCAM_ISO; + static const char* kCAM_SHUTTERSPD; + static const char* kCAM_APERTURE; + static const char* kCAM_WBMODE; + static const char* kCAM_MODE; + +signals: + void infoChanged (); + void videoStatusChanged (); + void photoStatusChanged (); + void photoModeChanged (); + void photoLapseChanged (); + void photoLapseCountChanged (); + void cameraModeChanged (); + void activeSettingsChanged (); + void storageFreeChanged (); + void storageTotalChanged (); + void batteryRemainingChanged (); + void dataReady (QByteArray data); + void parametersReady (); + void zoomLevelChanged (); + void focusLevelChanged (); + void streamsChanged (); + void currentStreamChanged (); + void thermalStreamChanged (); + void autoStreamChanged (); + void recordTimeChanged (); + void streamLabelsChanged (); + void thermalModeChanged (); + void thermalOpacityChanged (); + void storageStatusChanged (); + +protected: + virtual void _setVideoStatus (VideoStatus status); + virtual void _setPhotoStatus (PhotoStatus status); + virtual void _setCameraMode (CameraMode mode); + virtual void _requestStreamInfo (uint8_t streamID); + virtual void _requestStreamStatus (uint8_t streamID); + virtual QGCVideoStreamInfo* _findStream (uint8_t streamID, bool report = true); + virtual QGCVideoStreamInfo* _findStream (const QString name); + +protected slots: + virtual void _initWhenReady (); + virtual void _requestCameraSettings (); + virtual void _requestAllParameters (); + virtual void _requestParamUpdates (); + virtual void _requestCaptureStatus (); + virtual void _requestStorageInfo (); + virtual void _downloadFinished (); + virtual void _mavCommandResult (int vehicleId, int component, int command, int result, bool noReponseFromVehicle); + virtual void _dataReady (QByteArray data); + virtual void _paramDone (); + virtual void _streamTimeout (); + virtual void _streamStatusTimeout (); + virtual void _recTimerHandler (); + virtual void _checkForVideoStreams (); + +private: + bool _handleLocalization (QByteArray& bytes); + bool _replaceLocaleStrings (const QDomNode node, QByteArray& bytes); + bool _loadCameraDefinitionFile (QByteArray& bytes); + bool _loadConstants (const QDomNodeList nodeList); + bool _loadSettings (const QDomNodeList nodeList); + void _processRanges (); + bool _processCondition (const QString condition); + bool _processConditionTest (const QString conditionTest); + bool _loadNameValue (QDomNode option, const QString factName, FactMetaData* metaData, QString& optName, QString& optValue, QVariant& optVariant); + bool _loadRanges (QDomNode option, const QString factName, QString paramValue); + void _updateActiveList (); + void _updateRanges (Fact* pFact); + void _httpRequest (const QString& url); + void _handleDefinitionFile (const QString& url); + void _ftpDownloadComplete (const QString& fileName, const QString& errorMsg); + + QStringList _loadExclusions (QDomNode option); + QStringList _loadUpdates (QDomNode option); + QString _getParamName (const char* param_id); + +protected: + Vehicle* _vehicle = nullptr; + int _compID = 0; + mavlink_camera_information_t _info; + int _version = 0; + bool _cached = false; + bool _paramComplete = false; + qreal _zoomLevel = 0.0; + qreal _focusLevel = 0.0; + uint32_t _storageFree = 0; + uint32_t _storageTotal = 0; + int _batteryRemaining = -1; + QNetworkAccessManager* _netManager = nullptr; + QString _modelName; + QString _vendor; + QString _cacheFile; + CameraMode _cameraMode = CAM_MODE_UNDEFINED; + StorageStatus _storageStatus = STORAGE_NOT_SUPPORTED; + PhotoMode _photoMode = PHOTO_CAPTURE_SINGLE; + qreal _photoLapse = 1.0; + int _photoLapseCount = 0; + VideoStatus _video_status = VIDEO_CAPTURE_STATUS_UNDEFINED; + PhotoStatus _photo_status = PHOTO_CAPTURE_STATUS_UNDEFINED; + QStringList _activeSettings; + QStringList _settings; + QTimer _captureStatusTimer; + QList _valueExclusions; + QList _optionRanges; + QMap _originalOptNames; + QMap _originalOptValues; + QMap _paramIO; + int _storageInfoRetries = 0; + int _captureInfoRetries = 0; + bool _resetting = false; + QTimer _recTimer; + QTime _recTime; + uint32_t _recordTime = 0; + //-- Parameters that require a full update + QMap _requestUpdates; + QStringList _updatesToRequest; + //-- Video Streams + int _requestCount = 0; + int _currentStream = 0; + int _expectedCount = 1; + QTimer _streamInfoTimer; + QTimer _streamStatusTimer; + QmlObjectListModel _streams; + QStringList _streamLabels; + ThermalViewMode _thermalMode = THERMAL_BLEND; + double _thermalOpacity = 85.0; +}; diff --git a/src/Camera/QGCCameraIO.cc b/src/Camera/QGCCameraIO.cc new file mode 100644 index 0000000..13d3837 --- /dev/null +++ b/src/Camera/QGCCameraIO.cc @@ -0,0 +1,372 @@ + + +#include "QGCCameraControl.h" +#include "QGCCameraIO.h" + +QGC_LOGGING_CATEGORY(CameraIOLog, "CameraIOLog") +QGC_LOGGING_CATEGORY(CameraIOLogVerbose, "CameraIOLogVerbose") + +//----------------------------------------------------------------------------- +QGCCameraParamIO::QGCCameraParamIO(QGCCameraControl *control, Fact* fact, Vehicle *vehicle) + : QObject(control) + , _control(control) + , _fact(fact) + , _vehicle(vehicle) + , _sentRetries(0) + , _requestRetries(0) + , _done(false) + , _updateOnSet(false) + , _forceUIUpdate(false) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + _paramWriteTimer.setSingleShot(true); + _paramWriteTimer.setInterval(3000); + _paramRequestTimer.setSingleShot(true); + _paramRequestTimer.setInterval(3500); + if(_fact->writeOnly()) { + //-- Write mode is always "done" as it won't ever read + _done = true; + } else { + connect(&_paramRequestTimer, &QTimer::timeout, this, &QGCCameraParamIO::_paramRequestTimeout); + } + connect(&_paramWriteTimer, &QTimer::timeout, this, &QGCCameraParamIO::_paramWriteTimeout); + connect(_fact, &Fact::rawValueChanged, this, &QGCCameraParamIO::_factChanged); + connect(_fact, &Fact::_containerRawValueChanged, this, &QGCCameraParamIO::_containerRawValueChanged); + _pMavlink = qgcApp()->toolbox()->mavlinkProtocol(); + //-- TODO: Even though we don't use anything larger than 32-bit, this should + // probably be updated. + switch (_fact->type()) { + case FactMetaData::valueTypeUint8: + case FactMetaData::valueTypeBool: + _mavParamType = MAV_PARAM_EXT_TYPE_UINT8; + break; + case FactMetaData::valueTypeInt8: + _mavParamType = MAV_PARAM_EXT_TYPE_INT8; + break; + case FactMetaData::valueTypeUint16: + _mavParamType = MAV_PARAM_EXT_TYPE_UINT16; + break; + case FactMetaData::valueTypeInt16: + _mavParamType = MAV_PARAM_EXT_TYPE_INT16; + break; + case FactMetaData::valueTypeUint32: + _mavParamType = MAV_PARAM_EXT_TYPE_UINT32; + break; + case FactMetaData::valueTypeUint64: + _mavParamType = MAV_PARAM_EXT_TYPE_UINT64; + break; + case FactMetaData::valueTypeInt64: + _mavParamType = MAV_PARAM_EXT_TYPE_INT64; + break; + case FactMetaData::valueTypeFloat: + _mavParamType = MAV_PARAM_EXT_TYPE_REAL32; + break; + case FactMetaData::valueTypeDouble: + _mavParamType = MAV_PARAM_EXT_TYPE_REAL64; + break; + //-- String and custom are the same for now + case FactMetaData::valueTypeString: + case FactMetaData::valueTypeCustom: + _mavParamType = MAV_PARAM_EXT_TYPE_CUSTOM; + break; + default: + qWarning() << "Unsupported fact type" << _fact->type() << "for" << _fact->name(); + case FactMetaData::valueTypeInt32: + _mavParamType = MAV_PARAM_EXT_TYPE_INT32; + break; + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::setParamRequest() +{ + if(!_fact->writeOnly()) { + _paramRequestReceived = false; + _requestRetries = 0; + _paramRequestTimer.start(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_factChanged(QVariant value) +{ + if(!_forceUIUpdate) { + Q_UNUSED(value); + qCDebug(CameraIOLog) << "UI Fact" << _fact->name() << "changed to" << value; + _control->factChanged(_fact); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_containerRawValueChanged(const QVariant value) +{ + if(!_fact->readOnly()) { + Q_UNUSED(value); + qCDebug(CameraIOLog) << "Update Fact from camera" << _fact->name(); + _sentRetries = 0; + _sendParameter(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::sendParameter(bool updateUI) +{ + qCDebug(CameraIOLog) << "Send Fact" << _fact->name(); + _sentRetries = 0; + _updateOnSet = updateUI; + _sendParameter(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_sendParameter() +{ + WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); + if (!weakLink.expired()) { + SharedLinkInterfacePtr sharedLink = weakLink.lock(); + + mavlink_param_ext_set_t p; + memset(&p, 0, sizeof(mavlink_param_ext_set_t)); + param_ext_union_t union_value; + mavlink_message_t msg; + FactMetaData::ValueType_t factType = _fact->type(); + p.param_type = _mavParamType; + switch (factType) { + case FactMetaData::valueTypeUint8: + case FactMetaData::valueTypeBool: + union_value.param_uint8 = static_cast(_fact->rawValue().toUInt()); + break; + case FactMetaData::valueTypeInt8: + union_value.param_int8 = static_cast(_fact->rawValue().toInt()); + break; + case FactMetaData::valueTypeUint16: + union_value.param_uint16 = static_cast(_fact->rawValue().toUInt()); + break; + case FactMetaData::valueTypeInt16: + union_value.param_int16 = static_cast(_fact->rawValue().toInt()); + break; + case FactMetaData::valueTypeUint32: + union_value.param_uint32 = static_cast(_fact->rawValue().toUInt()); + break; + case FactMetaData::valueTypeInt64: + union_value.param_int64 = static_cast(_fact->rawValue().toLongLong()); + break; + case FactMetaData::valueTypeUint64: + union_value.param_uint64 = static_cast(_fact->rawValue().toULongLong()); + break; + case FactMetaData::valueTypeFloat: + union_value.param_float = _fact->rawValue().toFloat(); + break; + case FactMetaData::valueTypeDouble: + union_value.param_double = _fact->rawValue().toDouble(); + break; + //-- String and custom are the same for now + case FactMetaData::valueTypeString: + case FactMetaData::valueTypeCustom: + { + QByteArray custom = _fact->rawValue().toByteArray(); + memcpy(union_value.bytes, custom.data(), static_cast(std::max(custom.size(), static_cast(MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN)))); + } + break; + default: + qCritical() << "Unsupported fact type" << factType << "for" << _fact->name(); + case FactMetaData::valueTypeInt32: + union_value.param_int32 = static_cast(_fact->rawValue().toInt()); + break; + } + memcpy(&p.param_value[0], &union_value.bytes[0], MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); + p.target_system = static_cast(_vehicle->id()); + p.target_component = static_cast(_control->compID()); + strncpy(p.param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN); + mavlink_msg_param_ext_set_encode_chan( + static_cast(_pMavlink->getSystemId()), + static_cast(_pMavlink->getComponentId()), + sharedLink->mavlinkChannel(), + &msg, + &p); + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } + _paramWriteTimer.start(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_paramWriteTimeout() +{ + if(++_sentRetries > 3) { + qCWarning(CameraIOLog) << "No response for param set:" << _fact->name(); + _updateOnSet = false; + } else { + //-- Send it again + qCDebug(CameraIOLog) << "Param set retry:" << _fact->name() << _sentRetries; + _sendParameter(); + _paramWriteTimer.start(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::handleParamAck(const mavlink_param_ext_ack_t& ack) +{ + _paramWriteTimer.stop(); + if(ack.param_result == PARAM_ACK_ACCEPTED) { + QVariant val = _valueFromMessage(ack.param_value, ack.param_type); + if(_fact->rawValue() != val) { + _fact->_containerSetRawValue(val); + if(_updateOnSet) { + _updateOnSet = false; + _control->factChanged(_fact); + } + } + } else if(ack.param_result == PARAM_ACK_IN_PROGRESS) { + //-- Wait a bit longer for this one + qCDebug(CameraIOLogVerbose) << "Param set in progress:" << _fact->name(); + _paramWriteTimer.start(); + } else { + if(ack.param_result == PARAM_ACK_FAILED) { + if(++_sentRetries < 3) { + //-- Try again + qCWarning(CameraIOLog) << "Param set failed:" << _fact->name() << _sentRetries; + _paramWriteTimer.start(); + } + return; + } else if(ack.param_result == PARAM_ACK_VALUE_UNSUPPORTED) { + qCWarning(CameraIOLog) << "Param set unsuported:" << _fact->name(); + } + //-- If UI changed and value was not set, restore UI + QVariant val = _valueFromMessage(ack.param_value, ack.param_type); + if(_fact->rawValue() != val) { + if(_control->validateParameter(_fact, val)) { + _fact->_containerSetRawValue(val); + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::handleParamValue(const mavlink_param_ext_value_t& value) +{ + _paramRequestTimer.stop(); + QVariant newValue = _valueFromMessage(value.param_value, value.param_type); + if(_control->incomingParameter(_fact, newValue)) { + _fact->_containerSetRawValue(newValue); + } + _paramRequestReceived = true; + if(_forceUIUpdate) { + emit _fact->rawValueChanged(_fact->rawValue()); + emit _fact->valueChanged(_fact->rawValue()); + _forceUIUpdate = false; + } + if(!_done) { + _done = true; + _control->_paramDone(); + } + qCDebug(CameraIOLog) << QString("handleParamValue() %1 %2").arg(_fact->name()).arg(_fact->rawValueString()); +} + +//----------------------------------------------------------------------------- +QVariant +QGCCameraParamIO::_valueFromMessage(const char* value, uint8_t param_type) +{ + QVariant var; + param_ext_union_t u; + memcpy(u.bytes, value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN); + switch (param_type) { + case MAV_PARAM_EXT_TYPE_REAL32: + var = QVariant(u.param_float); + break; + case MAV_PARAM_EXT_TYPE_UINT8: + var = QVariant(u.param_uint8); + break; + case MAV_PARAM_EXT_TYPE_INT8: + var = QVariant(u.param_int8); + break; + case MAV_PARAM_EXT_TYPE_UINT16: + var = QVariant(u.param_uint16); + break; + case MAV_PARAM_EXT_TYPE_INT16: + var = QVariant(u.param_int16); + break; + case MAV_PARAM_EXT_TYPE_UINT32: + var = QVariant(u.param_uint32); + break; + case MAV_PARAM_EXT_TYPE_INT32: + var = QVariant(u.param_int32); + break; + case MAV_PARAM_EXT_TYPE_UINT64: + var = QVariant(static_cast(u.param_uint64)); + break; + case MAV_PARAM_EXT_TYPE_INT64: + var = QVariant(static_cast(u.param_int64)); + break; + case MAV_PARAM_EXT_TYPE_CUSTOM: + var = QVariant(QByteArray(value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN)); + break; + default: + var = QVariant(0); + qCritical() << "Invalid param_type used for camera setting:" << param_type; + } + return var; +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_paramRequestTimeout() +{ + if(++_requestRetries > 3) { + qCWarning(CameraIOLog) << "No response for param request:" << _fact->name(); + if(!_done) { + _done = true; + _control->_paramDone(); + } + } else { + //-- Request it again + qCDebug(CameraIOLog) << "Param request retry:" << _fact->name(); + paramRequest(false); + _paramRequestTimer.start(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::paramRequest(bool reset) +{ + //-- If it's write only, we don't request it. + if(_fact->writeOnly()) { + if(!_done) { + _done = true; + _control->_paramDone(); + } + return; + } + if(reset) { + _requestRetries = 0; + _forceUIUpdate = true; + } + qCDebug(CameraIOLog) << "Request parameter:" << _fact->name(); + WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); + if (!weakLink.expired()) { + SharedLinkInterfacePtr sharedLink = weakLink.lock(); + + char param_id[MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]; + memset(param_id, 0, sizeof(param_id)); + strncpy(param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN); + mavlink_message_t msg; + mavlink_msg_param_ext_request_read_pack_chan( + static_cast(_pMavlink->getSystemId()), + static_cast(_pMavlink->getComponentId()), + sharedLink->mavlinkChannel(), + &msg, + static_cast(_vehicle->id()), + static_cast(_control->compID()), + param_id, + -1); + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } + _paramRequestTimer.start(); +} diff --git a/src/Camera/QGCCameraIO.h b/src/Camera/QGCCameraIO.h new file mode 100644 index 0000000..8db7d15 --- /dev/null +++ b/src/Camera/QGCCameraIO.h @@ -0,0 +1,72 @@ + +#pragma once + +#include "QGCApplication.h" +#include + +class QGCCameraControl; + +Q_DECLARE_LOGGING_CATEGORY(CameraIOLog) +Q_DECLARE_LOGGING_CATEGORY(CameraIOLogVerbose) + +MAVPACKED( +typedef struct { + union { + float param_float; + double param_double; + int64_t param_int64; + uint64_t param_uint64; + int32_t param_int32; + uint32_t param_uint32; + int16_t param_int16; + uint16_t param_uint16; + int8_t param_int8; + uint8_t param_uint8; + uint8_t bytes[MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN]; + }; + uint8_t type; +}) param_ext_union_t; + +//----------------------------------------------------------------------------- +/// Camera parameter handler. +class QGCCameraParamIO : public QObject +{ +public: + QGCCameraParamIO(QGCCameraControl* control, Fact* fact, Vehicle* vehicle); + + void handleParamAck (const mavlink_param_ext_ack_t& ack); + void handleParamValue (const mavlink_param_ext_value_t& value); + void setParamRequest (); + bool paramDone () const { return _done; } + void paramRequest (bool reset = true); + void sendParameter (bool updateUI = false); + + QStringList optNames; + QVariantList optVariants; + +private slots: + void _paramWriteTimeout (); + void _paramRequestTimeout (); + void _factChanged (QVariant value); + void _containerRawValueChanged (const QVariant value); + +private: + void _sendParameter (); + QVariant _valueFromMessage (const char* value, uint8_t param_type); + +private: + QGCCameraControl* _control; + Fact* _fact; + Vehicle* _vehicle; + int _sentRetries; + int _requestRetries; + bool _paramRequestReceived; + QTimer _paramWriteTimer; + QTimer _paramRequestTimer; + bool _done; + bool _updateOnSet; + MAV_PARAM_EXT_TYPE _mavParamType; + MAVLinkProtocol* _pMavlink; + bool _forceUIUpdate; +}; + diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc new file mode 100644 index 0000000..ed53c21 --- /dev/null +++ b/src/Camera/QGCCameraManager.cc @@ -0,0 +1,518 @@ + + +#include "QGCApplication.h" +#include "QGCCameraManager.h" +#include "JoystickManager.h" + +QGC_LOGGING_CATEGORY(CameraManagerLog, "CameraManagerLog") + +//----------------------------------------------------------------------------- +QGCCameraManager::CameraStruct::CameraStruct(QObject* parent, uint8_t compID_) + : QObject(parent) + , compID(compID_) +{ +} + +//----------------------------------------------------------------------------- +QGCCameraManager::QGCCameraManager(Vehicle *vehicle) + : _vehicle(vehicle) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + qCDebug(CameraManagerLog) << "QGCCameraManager Created"; + connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &QGCCameraManager::_vehicleReady); + connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &QGCCameraManager::_mavlinkMessageReceived); + connect(&_cameraTimer, &QTimer::timeout, this, &QGCCameraManager::_cameraTimeout); + _cameraTimer.setSingleShot(false); + _lastZoomChange.start(); + _lastCameraChange.start(); + _cameraTimer.start(500); +} + +//----------------------------------------------------------------------------- +QGCCameraManager::~QGCCameraManager() +{ +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::setCurrentCamera(int sel) +{ + if(sel != _currentCamera && sel >= 0 && sel < _cameras.count()) { + _currentCamera = sel; + emit currentCameraChanged(); + emit streamChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_vehicleReady(bool ready) +{ + qCDebug(CameraManagerLog) << "_vehicleReady(" << ready << ")"; + if(ready) { + if(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle() == _vehicle) { + _vehicleReadyState = true; + JoystickManager *pJoyMgr = qgcApp()->toolbox()->joystickManager(); + _activeJoystickChanged(pJoyMgr->activeJoystick()); + connect(pJoyMgr, &JoystickManager::activeJoystickChanged, this, &QGCCameraManager::_activeJoystickChanged); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t& message) +{ + //-- Only pay attention to camera components, as identified by their compId + if(message.sysid == _vehicle->id() && (message.compid >= MAV_COMP_ID_CAMERA && message.compid <= MAV_COMP_ID_CAMERA6)) { + switch (message.msgid) { + case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS: + _handleCaptureStatus(message); + break; + case MAVLINK_MSG_ID_STORAGE_INFORMATION: + _handleStorageInfo(message); + break; + 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; + case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION: + _handleVideoStreamInfo(message); + break; + case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS: + _handleVideoStreamStatus(message); + break; + case MAVLINK_MSG_ID_BATTERY_STATUS: + _handleBatteryStatus(message); + break; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleHeartbeat(const mavlink_message_t &message) +{ + //-- First time hearing from this one? + QString sCompID = QString::number(message.compid); + if(!_cameraInfoRequest.contains(sCompID)) { + qCDebug(CameraManagerLog) << "Hearbeat from " << message.compid; + CameraStruct* pInfo = new CameraStruct(this, message.compid); + pInfo->lastHeartbeat.start(); + _cameraInfoRequest[sCompID] = pInfo; + //-- Request camera info + _requestCameraInfo(message.compid); + } else { + if(_cameraInfoRequest[sCompID]) { + CameraStruct* pInfo = _cameraInfoRequest[sCompID]; + //-- Check if we have indeed received the camera info + if(pInfo->infoReceived) { + //-- We have it. Just update the heartbeat timeout + pInfo->lastHeartbeat.start(); + } else { + //-- Try again. Maybe. + if(pInfo->lastHeartbeat.elapsed() > 2000) { + if(pInfo->tryCount > 10) { + if(!pInfo->gaveUp) { + pInfo->gaveUp = true; + qWarning() << "Giving up requesting camera info from" << _vehicle->id() << message.compid; + } + } else { + pInfo->tryCount++; + //-- Request camera info again. + _requestCameraInfo(message.compid); + } + } + } + } else { + qWarning() << "_cameraInfoRequest[" << sCompID << "] is null"; + } + } +} + +//----------------------------------------------------------------------------- +QGCCameraControl* +QGCCameraManager::currentCameraInstance() +{ + if(_currentCamera < _cameras.count() && _cameras.count()) { + QGCCameraControl* pCamera = qobject_cast(_cameras[_currentCamera]); + return pCamera; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraManager::currentStreamInstance() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + QGCVideoStreamInfo* pInfo = pCamera->currentStreamInstance(); + return pInfo; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraManager::thermalStreamInstance() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + QGCVideoStreamInfo* pInfo = pCamera->thermalStreamInstance(); + return pInfo; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCCameraControl* +QGCCameraManager::_findCamera(int id) +{ + for(int i = 0; i < _cameras.count(); i++) { + if(_cameras[i]) { + QGCCameraControl* pCamera = qobject_cast(_cameras[i]); + if(pCamera) { + if(pCamera->compID() == id) { + return pCamera; + } + } else { + qCritical() << "Null QGCCameraControl instance"; + } + } + } + //qWarning() << "Camera component id not found:" << id; + return nullptr; +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message) +{ + //-- Have we requested it? + QString sCompID = QString::number(message.compid); + if(_cameraInfoRequest.contains(sCompID) && !_cameraInfoRequest[sCompID]->infoReceived) { + //-- Flag it as done + _cameraInfoRequest[sCompID]->infoReceived = true; + mavlink_camera_information_t info; + mavlink_msg_camera_information_decode(&message, &info); + qCDebug(CameraManagerLog) << "_handleCameraInfo:" << reinterpret_cast(info.model_name) << reinterpret_cast(info.vendor_name) << "Comp ID:" << message.compid; + QGCCameraControl* pCamera = _vehicle->firmwarePlugin()->createCameraControl(&info, _vehicle, message.compid, this); + if(pCamera) { + QQmlEngine::setObjectOwnership(pCamera, QQmlEngine::CppOwnership); + _cameras.append(pCamera); + _cameraLabels << pCamera->modelName(); + emit camerasChanged(); + emit cameraLabelsChanged(); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_cameraTimeout() +{ + //-- Iterate cameras + foreach(QString sCompID, _cameraInfoRequest.keys()) { + if(_cameraInfoRequest[sCompID]) { + CameraStruct* pInfo = _cameraInfoRequest[sCompID]; + //-- Have we received a camera info message? + if(pInfo->infoReceived) { + //-- Has the camera stopped talking to us? + if(pInfo->lastHeartbeat.elapsed() > 5000) { + //-- Camera is gone. Remove it. + bool autoStream = false; + QGCCameraControl* pCamera = _findCamera(pInfo->compID); + if(pCamera) { + qWarning() << "Camera" << pCamera->modelName() << "stopped transmitting. Removing from list."; + int idx = _cameraLabels.indexOf(pCamera->modelName()); + if(idx >= 0) { + _cameraLabels.removeAt(idx); + } + idx = _cameras.indexOf(pCamera); + if(idx >= 0) { + _cameras.removeAt(idx); + } + autoStream = pCamera->autoStream(); + pCamera->deleteLater(); + delete pInfo; + } + _cameraInfoRequest.remove(sCompID); + emit cameraLabelsChanged(); + //-- If we have another camera, switch current camera. + if(_cameras.count()) { + setCurrentCamera(0); + } else { + //-- We're out of cameras + emit camerasChanged(); + if(autoStream) { + emit streamChanged(); + } + } + //-- Exit loop. + return; + } + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleCaptureStatus(const mavlink_message_t &message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_camera_capture_status_t cap; + mavlink_msg_camera_capture_status_decode(&message, &cap); + pCamera->handleCaptureStatus(cap); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleStorageInfo(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_storage_information_t st; + mavlink_msg_storage_information_decode(&message, &st); + pCamera->handleStorageInfo(st); + } +} + +//----------------------------------------------------------------------------- +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::_handleVideoStreamInfo(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_video_stream_information_t streamInfo; + mavlink_msg_video_stream_information_decode(&message, &streamInfo); + pCamera->handleVideoInfo(&streamInfo); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleVideoStreamStatus(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_video_stream_status_t streamStatus; + mavlink_msg_video_stream_status_decode(&message, &streamStatus); + pCamera->handleVideoStatus(&streamStatus); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleBatteryStatus(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_battery_status_t batteryStatus; + mavlink_msg_battery_status_decode(&message, &batteryStatus); + pCamera->handleBatteryStatus(batteryStatus); + } +} + +//----------------------------------------------------------------------------- +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 + 1); // Do Request + } +} + +//---------------------------------------------------------------------------------------- +void +QGCCameraManager::_activeJoystickChanged(Joystick* joystick) +{ + qCDebug(CameraManagerLog) << "Joystick changed"; + if(_activeJoystick) { + disconnect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom); + disconnect(_activeJoystick, &Joystick::startContinuousZoom, this, &QGCCameraManager::_startZoom); + disconnect(_activeJoystick, &Joystick::stopContinuousZoom, this, &QGCCameraManager::_stopZoom); + disconnect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera); + disconnect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream); + disconnect(_activeJoystick, &Joystick::triggerCamera, this, &QGCCameraManager::_triggerCamera); + disconnect(_activeJoystick, &Joystick::startVideoRecord, this, &QGCCameraManager::_startVideoRecording); + disconnect(_activeJoystick, &Joystick::stopVideoRecord, this, &QGCCameraManager::_stopVideoRecording); + disconnect(_activeJoystick, &Joystick::toggleVideoRecord, this, &QGCCameraManager::_toggleVideoRecording); + } + _activeJoystick = joystick; + if(_activeJoystick) { + connect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom); + connect(_activeJoystick, &Joystick::startContinuousZoom, this, &QGCCameraManager::_startZoom); + connect(_activeJoystick, &Joystick::stopContinuousZoom, this, &QGCCameraManager::_stopZoom); + connect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera); + connect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream); + connect(_activeJoystick, &Joystick::triggerCamera, this, &QGCCameraManager::_triggerCamera); + connect(_activeJoystick, &Joystick::startVideoRecord, this, &QGCCameraManager::_startVideoRecording); + connect(_activeJoystick, &Joystick::stopVideoRecord, this, &QGCCameraManager::_stopVideoRecording); + connect(_activeJoystick, &Joystick::toggleVideoRecord, this, &QGCCameraManager::_toggleVideoRecording); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_triggerCamera() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->takePhoto(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_startVideoRecording() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->startVideo(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stopVideoRecording() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->stopVideo(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_toggleVideoRecording() +{ + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->toggleVideo(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stepZoom(int direction) +{ + if(_lastZoomChange.elapsed() > 40) { + _lastZoomChange.start(); + qCDebug(CameraManagerLog) << "Step Camera Zoom" << direction; + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->stepZoom(direction); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_startZoom(int direction) +{ + qCDebug(CameraManagerLog) << "Start Camera Zoom" << direction; + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->startZoom(direction); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stopZoom() +{ + qCDebug(CameraManagerLog) << "Stop Camera Zoom"; + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + pCamera->stopZoom(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stepCamera(int direction) +{ + if(_lastCameraChange.elapsed() > 1000) { + _lastCameraChange.start(); + qCDebug(CameraManagerLog) << "Step Camera" << direction; + int c = _currentCamera + direction; + if(c < 0) c = _cameras.count() - 1; + if(c >= _cameras.count()) c = 0; + setCurrentCamera(c); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stepStream(int direction) +{ + if(_lastCameraChange.elapsed() > 1000) { + _lastCameraChange.start(); + QGCCameraControl* pCamera = currentCameraInstance(); + if(pCamera) { + qCDebug(CameraManagerLog) << "Step Camera Stream" << direction; + int c = pCamera->currentStream() + direction; + if(c < 0) c = pCamera->streams()->count() - 1; + if(c >= pCamera->streams()->count()) c = 0; + pCamera->setCurrentStream(c); + } + } +} + + diff --git a/src/Camera/QGCCameraManager.h b/src/Camera/QGCCameraManager.h new file mode 100644 index 0000000..5e51501 --- /dev/null +++ b/src/Camera/QGCCameraManager.h @@ -0,0 +1,112 @@ +/*! + * @file + * @brief Camera Controller + * @author Gus Grubba + * + */ + +/// @file +/// @brief MAVLink Camera API. Camera Manager. +/// @author Gus Grubba + +#pragma once + +#include "QGCApplication.h" +#include +#include "QmlObjectListModel.h" +#include "QGCCameraControl.h" + +#include +#include + +Q_DECLARE_LOGGING_CATEGORY(CameraManagerLog) + +class Joystick; + +//----------------------------------------------------------------------------- +/// Camera Manager +class QGCCameraManager : public QObject +{ + Q_OBJECT +public: + QGCCameraManager(Vehicle* vehicle); + virtual ~QGCCameraManager(); + + Q_PROPERTY(QmlObjectListModel* cameras READ cameras NOTIFY camerasChanged) + Q_PROPERTY(QStringList cameraLabels READ cameraLabels NOTIFY cameraLabelsChanged) + Q_PROPERTY(QGCCameraControl* currentCameraInstance READ currentCameraInstance NOTIFY currentCameraChanged) + Q_PROPERTY(int currentCamera READ currentCamera WRITE setCurrentCamera NOTIFY currentCameraChanged) + + //-- Return a list of cameras provided by this vehicle + virtual QmlObjectListModel* cameras () { return &_cameras; } + //-- Camera names to show the user (for selection) + virtual QStringList cameraLabels () { return _cameraLabels; } + //-- Current selected camera + virtual int currentCamera () { return _currentCamera; } + virtual QGCCameraControl* currentCameraInstance(); + //-- Set current camera + virtual void setCurrentCamera (int sel); + //-- Current stream + virtual QGCVideoStreamInfo* currentStreamInstance(); + //-- Current thermal stream + virtual QGCVideoStreamInfo* thermalStreamInstance(); + +signals: + void camerasChanged (); + void cameraLabelsChanged (); + void currentCameraChanged (); + void streamChanged (); + +protected slots: + virtual void _vehicleReady (bool ready); + virtual void _mavlinkMessageReceived (const mavlink_message_t& message); + virtual void _activeJoystickChanged (Joystick* joystick); + virtual void _stepZoom (int direction); + virtual void _startZoom (int direction); + virtual void _stopZoom (); + virtual void _stepCamera (int direction); + virtual void _stepStream (int direction); + virtual void _cameraTimeout (); + virtual void _triggerCamera (); + virtual void _startVideoRecording (); + virtual void _stopVideoRecording (); + virtual void _toggleVideoRecording (); + +protected: + virtual QGCCameraControl* _findCamera (int id); + virtual void _requestCameraInfo (int compID); + virtual void _handleHeartbeat (const mavlink_message_t& message); + virtual void _handleCameraInfo (const mavlink_message_t& message); + virtual void _handleStorageInfo (const mavlink_message_t& message); + virtual void _handleCameraSettings (const mavlink_message_t& message); + virtual void _handleParamAck (const mavlink_message_t& message); + virtual void _handleParamValue (const mavlink_message_t& message); + virtual void _handleCaptureStatus (const mavlink_message_t& message); + virtual void _handleVideoStreamInfo (const mavlink_message_t& message); + virtual void _handleVideoStreamStatus(const mavlink_message_t& message); + virtual void _handleBatteryStatus (const mavlink_message_t& message); + +protected: + + class CameraStruct : public QObject { + public: + CameraStruct(QObject* parent, uint8_t compID_); + QElapsedTimer lastHeartbeat; + bool infoReceived = false; + bool gaveUp = false; + int tryCount = 0; + uint8_t compID = 0; + }; + + Vehicle* _vehicle = nullptr; + Joystick* _activeJoystick = nullptr; + bool _vehicleReadyState = false; + int _currentTask = 0; + QmlObjectListModel _cameras; + QStringList _cameraLabels; + int _currentCamera = 0; + QElapsedTimer _lastZoomChange; + QElapsedTimer _lastCameraChange; + QTimer _cameraTimer; + QMap _cameraInfoRequest; +}; diff --git a/src/QGCCameraControl.cc b/src/QGCCameraControl.cc new file mode 100644 index 0000000..f85acb7 --- /dev/null +++ b/src/QGCCameraControl.cc @@ -0,0 +1,2259 @@ + + +#include "QGCCameraControl.h" +#include "QGCCameraIO.h" +#include "SettingsManager.h" +#include "VideoManager.h" +#include "QGCMapEngine.h" +#include "QGCCameraManager.h" +#include "FTPManager.h" +#include "QGCLZMA.h" + +#include +#include +#include +#include + +QGC_LOGGING_CATEGORY(CameraControlLog, "CameraControlLog") +QGC_LOGGING_CATEGORY(CameraControlVerboseLog, "CameraControlVerboseLog") + +static const char* kCondition = "condition"; +static const char* kControl = "control"; +static const char* kDefault = "default"; +static const char* kDefnition = "definition"; +static const char* kDescription = "description"; +static const char* kExclusion = "exclude"; +static const char* kExclusions = "exclusions"; +static const char* kLocale = "locale"; +static const char* kLocalization = "localization"; +static const char* kMax = "max"; +static const char* kMin = "min"; +static const char* kModel = "model"; +static const char* kName = "name"; +static const char* kOption = "option"; +static const char* kOptions = "options"; +static const char* kOriginal = "original"; +static const char* kParameter = "parameter"; +static const char* kParameterrange = "parameterrange"; +static const char* kParameterranges = "parameterranges"; +static const char* kParameters = "parameters"; +static const char* kReadOnly = "readonly"; +static const char* kWriteOnly = "writeonly"; +static const char* kRoption = "roption"; +static const char* kStep = "step"; +static const char* kDecimalPlaces = "decimalPlaces"; +static const char* kStrings = "strings"; +static const char* kTranslated = "translated"; +static const char* kType = "type"; +static const char* kUnit = "unit"; +static const char* kUpdate = "update"; +static const char* kUpdates = "updates"; +static const char* kValue = "value"; +static const char* kVendor = "vendor"; +static const char* kVersion = "version"; + +static const char* kPhotoMode = "PhotoMode"; +static const char* kPhotoLapse = "PhotoLapse"; +static const char* kPhotoLapseCount = "PhotoLapseCount"; +static const char* kThermalOpacity = "ThermalOpacity"; +static const char* kThermalMode = "ThermalMode"; + +//----------------------------------------------------------------------------- +// Known Parameters +const char* QGCCameraControl::kCAM_EV = "CAM_EV"; +const char* QGCCameraControl::kCAM_EXPMODE = "CAM_EXPMODE"; +const char* QGCCameraControl::kCAM_ISO = "CAM_ISO"; +const char* QGCCameraControl::kCAM_SHUTTERSPD = "CAM_SHUTTERSPD"; +const char* QGCCameraControl::kCAM_APERTURE = "CAM_APERTURE"; +const char* QGCCameraControl::kCAM_WBMODE = "CAM_WBMODE"; +const char* QGCCameraControl::kCAM_MODE = "CAM_MODE"; + +//----------------------------------------------------------------------------- +QGCCameraOptionExclusion::QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_) + : QObject(parent) + , param(param_) + , value(value_) + , exclusions(exclusions_) +{ +} + +//----------------------------------------------------------------------------- +QGCCameraOptionRange::QGCCameraOptionRange(QObject* parent, QString param_, QString value_, QString targetParam_, QString condition_, QStringList optNames_, QStringList optValues_) + : QObject(parent) + , param(param_) + , value(value_) + , targetParam(targetParam_) + , condition(condition_) + , optNames(optNames_) + , optValues(optValues_) +{ +} + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, bool& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue() != "0"; + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, int& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue().toInt(); + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, QString& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue(); + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_value(QDomNode& element, const char* tagName, QString& target) +{ + QDomElement de = element.firstChildElement(tagName); + if(de.isNull()) { + return false; + } + target = de.text(); + return true; +} + +//----------------------------------------------------------------------------- +QGCCameraControl::QGCCameraControl(const mavlink_camera_information_t *info, Vehicle* vehicle, int compID, QObject* parent) + : FactGroup(0, parent, true /* ignore camel case */) + , _vehicle(vehicle) + , _compID(compID) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + memcpy(&_info, info, sizeof(mavlink_camera_information_t)); + connect(this, &QGCCameraControl::dataReady, this, &QGCCameraControl::_dataReady); + _vendor = QString(reinterpret_cast(info->vendor_name)); + _modelName = QString(reinterpret_cast(info->model_name)); + int ver = static_cast(_info.cam_definition_version); + _cacheFile = QString::asprintf("%s/%s_%s_%03d.xml", + qgcApp()->toolbox()->settingsManager()->appSettings()->parameterSavePath().toStdString().c_str(), + _vendor.toStdString().c_str(), + _modelName.toStdString().c_str(), + ver); + if(info->cam_definition_uri[0] != 0) { + //-- Process camera definition file + _handleDefinitionFile(info->cam_definition_uri); + } else { + _initWhenReady(); + } + QSettings settings; + _photoMode = static_cast(settings.value(kPhotoMode, static_cast(PHOTO_CAPTURE_SINGLE)).toInt()); + _photoLapse = settings.value(kPhotoLapse, 1.0).toDouble(); + _photoLapseCount = settings.value(kPhotoLapseCount, 0).toInt(); + _thermalOpacity = settings.value(kThermalOpacity, 85.0).toDouble(); + _thermalMode = static_cast(settings.value(kThermalMode, static_cast(THERMAL_BLEND)).toUInt()); + _recTimer.setSingleShot(false); + _recTimer.setInterval(333); + connect(&_recTimer, &QTimer::timeout, this, &QGCCameraControl::_recTimerHandler); +} + +//----------------------------------------------------------------------------- +QGCCameraControl::~QGCCameraControl() +{ + delete _netManager; + _netManager = nullptr; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_initWhenReady() +{ + qCDebug(CameraControlLog) << "_initWhenReady()"; + if(isBasic()) { + qCDebug(CameraControlLog) << "Basic, MAVLink only messages."; + _requestCameraSettings(); + QTimer::singleShot(250, this, &QGCCameraControl::_checkForVideoStreams); + //-- Basic cameras have no parameters + _paramComplete = true; + emit parametersReady(); + } else { + _requestAllParameters(); + //-- Give some time to load the parameters before going after the camera settings + QTimer::singleShot(2000, this, &QGCCameraControl::_requestCameraSettings); + } + connect(_vehicle, &Vehicle::mavCommandResult, this, &QGCCameraControl::_mavCommandResult); + connect(&_captureStatusTimer, &QTimer::timeout, this, &QGCCameraControl::_requestCaptureStatus); + _captureStatusTimer.setSingleShot(true); + QTimer::singleShot(2500, this, &QGCCameraControl::_requestStorageInfo); + _captureStatusTimer.start(2750); + emit infoChanged(); + + delete _netManager; + _netManager = nullptr; +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::firmwareVersion() +{ + int major = (_info.firmware_version >> 24) & 0xFF; + int minor = (_info.firmware_version >> 16) & 0xFF; + int build = _info.firmware_version & 0xFFFF; + return QString::asprintf("%d.%d.%d", major, minor, build); +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::recordTimeStr() +{ + return QTime(0, 0).addMSecs(static_cast(recordTime())).toString("hh:mm:ss"); +} + +//----------------------------------------------------------------------------- +QGCCameraControl::VideoStatus +QGCCameraControl::videoStatus() +{ + return _video_status; +} + +//----------------------------------------------------------------------------- +QGCCameraControl::PhotoStatus +QGCCameraControl::photoStatus() +{ + return _photo_status; +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::storageFreeStr() +{ + return QGCMapEngine::storageFreeSizeToString(static_cast(_storageFree)); +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::batteryRemainingStr() +{ + if(_batteryRemaining >= 0) { + return QGCMapEngine::numberToString(static_cast(_batteryRemaining)) + " %"; + } + return ""; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setCameraMode(CameraMode mode) +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "setCameraMode(" << mode << ")"; + if(mode == CAM_MODE_VIDEO) { + setVideoMode(); + } else if(mode == CAM_MODE_PHOTO) { + setPhotoMode(); + } else { + qCDebug(CameraControlLog) << "setCameraMode() Invalid mode:" << mode; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoMode(PhotoMode mode) +{ + if(!_resetting) { + _photoMode = mode; + QSettings settings; + settings.setValue(kPhotoMode, static_cast(mode)); + emit photoModeChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoLapse(qreal interval) +{ + _photoLapse = interval; + QSettings settings; + settings.setValue(kPhotoLapse, interval); + emit photoLapseChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoLapseCount(int count) +{ + _photoLapseCount = count; + QSettings settings; + settings.setValue(kPhotoLapseCount, count); + emit photoLapseCountChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_setCameraMode(CameraMode mode) +{ + if(_cameraMode != mode) { + _cameraMode = mode; + emit cameraModeChanged(); + //-- Update stream status + _streamStatusTimer.start(1000); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::toggleMode() +{ + if(!_resetting) { + if(cameraMode() == CAM_MODE_PHOTO || cameraMode() == CAM_MODE_SURVEY) { + setVideoMode(); + } else if(cameraMode() == CAM_MODE_VIDEO) { + setPhotoMode(); + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::toggleVideo() +{ + if(!_resetting) { + if(videoStatus() == VIDEO_CAPTURE_STATUS_RUNNING) { + return stopVideo(); + } else { + return startVideo(); + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::takePhoto() +{ + qCDebug(CameraControlLog) << "takePhoto()"; + //-- Check if camera can capture photos or if it can capture it while in Video Mode + if(!capturesPhotos()) { + qCWarning(CameraControlLog) << "Camera does not handle image capture"; + return false; + } + if(cameraMode() == CAM_MODE_VIDEO && !photosInVideoMode()) { + qCWarning(CameraControlLog) << "Camera does not handle image capture while in video mode"; + return false; + } + if(photoStatus() != PHOTO_CAPTURE_IDLE) { + qCWarning(CameraControlLog) << "Camera not idle"; + return false; + } + if(!_resetting) { + if(capturesPhotos()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_IMAGE_START_CAPTURE, // Command id + false, // ShowError + 0, // Reserved (Set to 0) + static_cast(_photoMode == PHOTO_CAPTURE_SINGLE ? 0 : _photoLapse), // Duration between two consecutive pictures (in seconds--ignored if single image) + _photoMode == PHOTO_CAPTURE_SINGLE ? 1 : _photoLapseCount); // Number of images to capture total - 0 for unlimited capture + _setPhotoStatus(PHOTO_CAPTURE_IN_PROGRESS); + _captureInfoRetries = 0; + //-- Capture local image as well + if(qgcApp()->toolbox()->videoManager()) { + qgcApp()->toolbox()->videoManager()->grabImage(); + } + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::stopTakePhoto() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "stopTakePhoto()"; + if(photoStatus() == PHOTO_CAPTURE_IDLE || (photoStatus() != PHOTO_CAPTURE_INTERVAL_IDLE && photoStatus() != PHOTO_CAPTURE_INTERVAL_IN_PROGRESS)) { + return false; + } + if(capturesPhotos()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_IMAGE_STOP_CAPTURE, // Command id + false, // ShowError + 0); // Reserved (Set to 0) + _setPhotoStatus(PHOTO_CAPTURE_IDLE); + _captureInfoRetries = 0; + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::startVideo() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "startVideo()"; + //-- Check if camera can capture videos or if it can capture it while in Photo Mode + if(!capturesVideo() || (cameraMode() == CAM_MODE_PHOTO && !videoInPhotoMode())) { + return false; + } + if(videoStatus() != VIDEO_CAPTURE_STATUS_RUNNING) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_START_CAPTURE, // Command id + false, // Don't Show Error (handle locally) + 0, // Reserved (Set to 0) + 0); // CAMERA_CAPTURE_STATUS Frequency + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::stopVideo() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "stopVideo()"; + if(videoStatus() == VIDEO_CAPTURE_STATUS_RUNNING) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_STOP_CAPTURE, // Command id + false, // Don't Show Error (handle locally) + 0); // Reserved (Set to 0) + return true; + } + } + return false; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setVideoMode() +{ + if(!_resetting && hasModes()) { + qCDebug(CameraControlLog) << "setVideoMode()"; + //-- Does it have a mode parameter? + Fact* pMode = mode(); + if(pMode) { + if(cameraMode() != CAM_MODE_VIDEO) { + pMode->setRawValue(CAM_MODE_VIDEO); + _setCameraMode(CAM_MODE_VIDEO); + } + } else { + //-- Use MAVLink Command + if(_cameraMode != CAM_MODE_VIDEO) { + //-- Use basic MAVLink message + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_MODE, // Command id + true, // ShowError + 0, // Reserved (Set to 0) + CAM_MODE_VIDEO); // Camera mode (0: photo, 1: video) + _setCameraMode(CAM_MODE_VIDEO); + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoMode() +{ + if(!_resetting && hasModes()) { + qCDebug(CameraControlLog) << "setPhotoMode()"; + //-- Does it have a mode parameter? + Fact* pMode = mode(); + if(pMode) { + if(cameraMode() != CAM_MODE_PHOTO) { + pMode->setRawValue(CAM_MODE_PHOTO); + _setCameraMode(CAM_MODE_PHOTO); + } + } else { + //-- Use MAVLink Command + if(_cameraMode != CAM_MODE_PHOTO) { + //-- Use basic MAVLink message + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_MODE, // Command id + true, // ShowError + 0, // Reserved (Set to 0) + CAM_MODE_PHOTO); // Camera mode (0: photo, 1: video) + _setCameraMode(CAM_MODE_PHOTO); + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setThermalMode(ThermalViewMode mode) +{ + QSettings settings; + settings.setValue(kThermalMode, static_cast(mode)); + _thermalMode = mode; + emit thermalModeChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setThermalOpacity(double val) +{ + if(val < 0.0) val = 0.0; + if(val > 100.0) val = 100.0; + if(fabs(_thermalOpacity - val) > 0.1) { + _thermalOpacity = val; + QSettings settings; + settings.setValue(kThermalOpacity, val); + emit thermalOpacityChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setZoomLevel(qreal level) +{ + qCDebug(CameraControlLog) << "setZoomLevel()" << level; + if(hasZoom()) { + //-- Limit + level = std::min(std::max(level, 0.0), 100.0); + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_RANGE, // Zoom type + static_cast(level)); // Level + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setFocusLevel(qreal level) +{ + qCDebug(CameraControlLog) << "setFocusLevel()" << level; + if(hasFocus()) { + //-- Limit + level = std::min(std::max(level, 0.0), 100.0); + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_FOCUS, // Command id + false, // ShowError + FOCUS_TYPE_RANGE, // Focus type + static_cast(level)); // Level + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::resetSettings() +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "resetSettings()"; + _resetting = true; + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_RESET_CAMERA_SETTINGS, // Command id + true, // ShowError + 1); // Do Reset + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::formatCard(int id) +{ + if(!_resetting) { + qCDebug(CameraControlLog) << "formatCard()"; + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_STORAGE_FORMAT, // Command id + true, // ShowError + id, // Storage ID (1 for first, 2 for second, etc.) + 1); // Do Format + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::stepZoom(int direction) +{ + qCDebug(CameraControlLog) << "stepZoom()" << direction; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_STEP, // Zoom type + direction); // Direction (-1 wide, 1 tele) + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::startZoom(int direction) +{ + qCDebug(CameraControlLog) << "startZoom()" << direction; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_CONTINUOUS, // Zoom type + direction); // Direction (-1 wide, 1 tele) + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::stopZoom() +{ + qCDebug(CameraControlLog) << "stopZoom()"; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_CONTINUOUS, // Zoom type + 0); // Direction (-1 wide, 1 tele) + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestCaptureStatus() +{ + qCDebug(CameraControlLog) << "_requestCaptureStatus()"; + _vehicle->sendMavCommand( + _compID, // target component + MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS, // command id + false, // showError + 1); // Do Request +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::factChanged(Fact* pFact) +{ + _updateActiveList(); + _updateRanges(pFact); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle) +{ + //-- Is this ours? + if(_vehicle->id() != vehicleId || compID() != component) { + return; + } + if(!noReponseFromVehicle && result == MAV_RESULT_IN_PROGRESS) { + //-- Do Nothing + qCDebug(CameraControlLog) << "In progress response for" << command; + }else if(!noReponseFromVehicle && result == MAV_RESULT_ACCEPTED) { + switch(command) { + case MAV_CMD_RESET_CAMERA_SETTINGS: + _resetting = false; + if(isBasic()) { + _requestCameraSettings(); + } else { + QTimer::singleShot(500, this, &QGCCameraControl::_requestAllParameters); + QTimer::singleShot(2500, this, &QGCCameraControl::_requestCameraSettings); + } + break; + case MAV_CMD_VIDEO_START_CAPTURE: + _setVideoStatus(VIDEO_CAPTURE_STATUS_RUNNING); + _captureStatusTimer.start(1000); + break; + case MAV_CMD_VIDEO_STOP_CAPTURE: + _setVideoStatus(VIDEO_CAPTURE_STATUS_STOPPED); + _captureStatusTimer.start(1000); + break; + case MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS: + _captureInfoRetries = 0; + break; + case MAV_CMD_REQUEST_STORAGE_INFORMATION: + _storageInfoRetries = 0; + break; + case MAV_CMD_IMAGE_START_CAPTURE: + _captureStatusTimer.start(1000); + break; + } + } else { + if(noReponseFromVehicle || result == MAV_RESULT_TEMPORARILY_REJECTED || result == MAV_RESULT_FAILED) { + if(noReponseFromVehicle) { + qCDebug(CameraControlLog) << "No response for" << command; + } else if (result == MAV_RESULT_TEMPORARILY_REJECTED) { + qCDebug(CameraControlLog) << "Command temporarily rejected for" << command; + } else { + qCDebug(CameraControlLog) << "Command failed for" << command; + } + switch(command) { + case MAV_CMD_RESET_CAMERA_SETTINGS: + _resetting = false; + qCDebug(CameraControlLog) << "Failed to reset camera settings"; + break; + case MAV_CMD_IMAGE_START_CAPTURE: + case MAV_CMD_IMAGE_STOP_CAPTURE: + if(++_captureInfoRetries < 3) { + _captureStatusTimer.start(1000); + } else { + qCDebug(CameraControlLog) << "Giving up start/stop image capture"; + _setPhotoStatus(PHOTO_CAPTURE_IDLE); + } + break; + case MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS: + if(++_captureInfoRetries < 3) { + _captureStatusTimer.start(500); + } else { + qCDebug(CameraControlLog) << "Giving up requesting capture status"; + } + break; + case MAV_CMD_REQUEST_STORAGE_INFORMATION: + if(++_storageInfoRetries < 3) { + QTimer::singleShot(500, this, &QGCCameraControl::_requestStorageInfo); + } else { + qCDebug(CameraControlLog) << "Giving up requesting storage status"; + } + break; + } + } else { + qCDebug(CameraControlLog) << "Bad response for" << command << result; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_setVideoStatus(VideoStatus status) +{ + if(_video_status != status) { + _video_status = status; + emit videoStatusChanged(); + if(status == VIDEO_CAPTURE_STATUS_RUNNING) { + _recordTime = 0; + _recTime = QTime::currentTime(); + _recTimer.start(); + } else { + _recTimer.stop(); + _recordTime = 0; + emit recordTimeChanged(); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_recTimerHandler() +{ + _recordTime = static_cast(_recTime.msecsTo(QTime::currentTime())); + emit recordTimeChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_setPhotoStatus(PhotoStatus status) +{ + if(_photo_status != status) { + qCDebug(CameraControlLog) << "Set Photo Status:" << status; + _photo_status = status; + emit photoStatusChanged(); + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadCameraDefinitionFile(QByteArray& bytes) +{ + QByteArray originalData(bytes); + //-- Handle localization + if(!_handleLocalization(bytes)) { + return false; + } + int errorLine; + QString errorMsg; + QDomDocument doc; + if(!doc.setContent(bytes, false, &errorMsg, &errorLine)) { + qCCritical(CameraControlLog) << "Unable to parse camera definition file on line:" << errorLine; + qCCritical(CameraControlLog) << errorMsg; + return false; + } + //-- Load camera constants + QDomNodeList defElements = doc.elementsByTagName(kDefnition); + if(!defElements.size() || !_loadConstants(defElements)) { + qCWarning(CameraControlLog) << "Unable to load camera constants from camera definition"; + return false; + } + //-- Load camera parameters + QDomNodeList paramElements = doc.elementsByTagName(kParameters); + if(!paramElements.size()) { + qCDebug(CameraControlLog) << "No parameters to load from camera"; + return false; + } + if(!_loadSettings(paramElements)) { + qCWarning(CameraControlLog) << "Unable to load camera parameters from camera definition"; + return false; + } + //-- If this is new, cache it + if(!_cached) { + qCDebug(CameraControlLog) << "Saving camera definition file" << _cacheFile; + QFile file(_cacheFile); + if (!file.open(QIODevice::WriteOnly)) { + qWarning() << QString("Could not save cache file %1. Error: %2").arg(_cacheFile).arg(file.errorString()); + } else { + file.write(originalData); + } + } + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadConstants(const QDomNodeList nodeList) +{ + QDomNode node = nodeList.item(0); + if(!read_attribute(node, kVersion, _version)) { + return false; + } + if(!read_value(node, kModel, _modelName)) { + return false; + } + if(!read_value(node, kVendor, _vendor)) { + return false; + } + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadSettings(const QDomNodeList nodeList) +{ + QDomNode node = nodeList.item(0); + QDomElement elem = node.toElement(); + QDomNodeList parameters = elem.elementsByTagName(kParameter); + //-- Pre-process settings (maintain order and skip non-controls) + for(int i = 0; i < parameters.size(); i++) { + QDomNode parameterNode = parameters.item(i); + QString name; + if(read_attribute(parameterNode, kName, name)) { + bool control = true; + read_attribute(parameterNode, kControl, control); + if(control) { + _settings << name; + } + } else { + qCritical() << "Parameter entry missing parameter name"; + return false; + } + } + //-- Load parameters + for(int i = 0; i < parameters.size(); i++) { + QDomNode parameterNode = parameters.item(i); + QString factName; + read_attribute(parameterNode, kName, factName); + QString type; + if(!read_attribute(parameterNode, kType, type)) { + qCritical() << QString("Parameter %1 missing parameter type").arg(factName); + return false; + } + //-- Does it have a control? + bool control = true; + read_attribute(parameterNode, kControl, control); + //-- Is it read only? + bool readOnly = false; + read_attribute(parameterNode, kReadOnly, readOnly); + //-- Is it write only? + bool writeOnly = false; + read_attribute(parameterNode, kWriteOnly, writeOnly); + //-- It can't be both + if(readOnly && writeOnly) { + qCritical() << QString("Parameter %1 cannot be both read only and write only").arg(factName); + } + //-- Param type + bool unknownType; + FactMetaData::ValueType_t factType = FactMetaData::stringToType(type, unknownType); + if (unknownType) { + qCritical() << QString("Unknown type for parameter %1").arg(factName); + return false; + } + //-- By definition, custom types do not have control + if(factType == FactMetaData::valueTypeCustom) { + control = false; + } + //-- Description + QString description; + if(!read_value(parameterNode, kDescription, description)) { + qCritical() << QString("Parameter %1 missing parameter description").arg(factName); + return false; + } + //-- Check for updates + QStringList updates = _loadUpdates(parameterNode); + if(updates.size()) { + qCDebug(CameraControlVerboseLog) << "Parameter" << factName << "requires updates for:" << updates; + _requestUpdates[factName] = updates; + } + //-- Build metadata + FactMetaData* metaData = new FactMetaData(factType, factName, this); + QQmlEngine::setObjectOwnership(metaData, QQmlEngine::CppOwnership); + metaData->setShortDescription(description); + metaData->setLongDescription(description); + metaData->setHasControl(control); + metaData->setReadOnly(readOnly); + metaData->setWriteOnly(writeOnly); + //-- Options (enums) + QDomElement optionElem = parameterNode.toElement(); + QDomNodeList optionsRoot = optionElem.elementsByTagName(kOptions); + if(optionsRoot.size()) { + //-- Iterate options + QDomNode node = optionsRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList options = elem.elementsByTagName(kOption); + for(int i = 0; i < options.size(); i++) { + QDomNode option = options.item(i); + QString optName; + QString optValue; + QVariant optVariant; + if(!_loadNameValue(option, factName, metaData, optName, optValue, optVariant)) { + delete metaData; + return false; + } + metaData->addEnumInfo(optName, optVariant); + _originalOptNames[factName] << optName; + _originalOptValues[factName] << optVariant; + //-- Check for exclusions + QStringList exclusions = _loadExclusions(option); + if(exclusions.size()) { + qCDebug(CameraControlVerboseLog) << "New exclusions:" << factName << optValue << exclusions; + QGCCameraOptionExclusion* pExc = new QGCCameraOptionExclusion(this, factName, optValue, exclusions); + QQmlEngine::setObjectOwnership(pExc, QQmlEngine::CppOwnership); + _valueExclusions.append(pExc); + } + //-- Check for range rules + if(!_loadRanges(option, factName, optValue)) { + delete metaData; + return false; + } + } + } + QString defaultValue; + if(read_attribute(parameterNode, kDefault, defaultValue)) { + QVariant defaultVariant; + QString errorString; + if (metaData->convertAndValidateRaw(defaultValue, false, defaultVariant, errorString)) { + metaData->setRawDefaultValue(defaultVariant); + } else { + qWarning() << "Invalid default value for" << factName + << " type:" << metaData->type() + << " value:" << defaultValue + << " error:" << errorString; + } + } + //-- Set metadata and Fact + if (_nameToFactMetaDataMap.contains(factName)) { + qWarning() << QStringLiteral("Duplicate fact name:") << factName; + delete metaData; + } else { + { + //-- Check for Min Value + QString attr; + if(read_attribute(parameterNode, kMin, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setRawMin(typedValue); + } else { + qWarning() << "Invalid min value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Max Value + QString attr; + if(read_attribute(parameterNode, kMax, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setRawMax(typedValue); + } else { + qWarning() << "Invalid max value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Step Value + QString attr; + if(read_attribute(parameterNode, kStep, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setRawIncrement(typedValue.toDouble()); + } else { + qWarning() << "Invalid step value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Decimal Places + QString attr; + if(read_attribute(parameterNode, kDecimalPlaces, attr)) { + QVariant typedValue; + QString errorString; + if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { + metaData->setDecimalPlaces(typedValue.toInt()); + } else { + qWarning() << "Invalid decimal places value for" << factName + << " type:" << metaData->type() + << " value:" << attr + << " error:" << errorString; + } + } + } + { + //-- Check for Units + QString attr; + if(read_attribute(parameterNode, kUnit, attr)) { + metaData->setRawUnits(attr); + } + } + qCDebug(CameraControlLog) << "New parameter:" << factName << (readOnly ? "ReadOnly" : "Writable") << (writeOnly ? "WriteOnly" : "Readable"); + _nameToFactMetaDataMap[factName] = metaData; + Fact* pFact = new Fact(_compID, factName, factType, this); + QQmlEngine::setObjectOwnership(pFact, QQmlEngine::CppOwnership); + pFact->setMetaData(metaData); + pFact->_containerSetRawValue(metaData->rawDefaultValue()); + QGCCameraParamIO* pIO = new QGCCameraParamIO(this, pFact, _vehicle); + QQmlEngine::setObjectOwnership(pIO, QQmlEngine::CppOwnership); + _paramIO[factName] = pIO; + _addFact(pFact, factName); + } + } + if(_nameToFactMetaDataMap.size() > 0) { + _addFactGroup(this, "camera"); + _processRanges(); + _activeSettings = _settings; + emit activeSettingsChanged(); + return true; + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_handleLocalization(QByteArray& bytes) +{ + QString errorMsg; + int errorLine; + QDomDocument doc; + if(!doc.setContent(bytes, false, &errorMsg, &errorLine)) { + qCritical() << "Unable to parse camera definition file on line:" << errorLine; + qCritical() << errorMsg; + return false; + } + //-- Find out where we are + QLocale locale = QLocale::system(); +#if defined (Q_OS_MAC) + locale = QLocale(locale.name()); +#endif + QString localeName = locale.name().toLower().replace("-", "_"); + qCDebug(CameraControlLog) << "Current locale:" << localeName; + if(localeName == "en_us") { + // Nothing to do + return true; + } + QDomNodeList locRoot = doc.elementsByTagName(kLocalization); + if(!locRoot.size()) { + // Nothing to do + return true; + } + //-- Iterate locales + QDomNode node = locRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList locales = elem.elementsByTagName(kLocale); + for(int i = 0; i < locales.size(); i++) { + QDomNode locale = locales.item(i); + QString name; + if(!read_attribute(locale, kName, name)) { + qWarning() << "Localization entry is missing its name attribute"; + continue; + } + // If we found a direct match, deal with it now + if(localeName == name.toLower().replace("-", "_")) { + return _replaceLocaleStrings(locale, bytes); + } + } + //-- No direct match. Pick first matching language (if any) + localeName = localeName.left(3); + for(int i = 0; i < locales.size(); i++) { + QDomNode locale = locales.item(i); + QString name; + read_attribute(locale, kName, name); + if(name.toLower().startsWith(localeName)) { + return _replaceLocaleStrings(locale, bytes); + } + } + //-- Could not find a language to use + qWarning() << "No match for" << QLocale::system().name() << "in camera definition file"; + //-- Just use default, en_US + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_replaceLocaleStrings(const QDomNode node, QByteArray& bytes) +{ + QDomElement stringElem = node.toElement(); + QDomNodeList strings = stringElem.elementsByTagName(kStrings); + for(int i = 0; i < strings.size(); i++) { + QDomNode stringNode = strings.item(i); + QString original; + QString translated; + if(read_attribute(stringNode, kOriginal, original)) { + if(read_attribute(stringNode, kTranslated, translated)) { + QString o; o = "\"" + original + "\""; + QString t; t = "\"" + translated + "\""; + bytes.replace(o.toUtf8(), t.toUtf8()); + o = ">" + original + "<"; + t = ">" + translated + "<"; + bytes.replace(o.toUtf8(), t.toUtf8()); + } + } + } + return true; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestAllParameters() +{ + //-- Reset receive list + for(const QString& paramName: _paramIO.keys()) { + if(_paramIO[paramName]) { + _paramIO[paramName]->setParamRequest(); + } else { + qCritical() << "QGCParamIO is NULL" << paramName; + } + } + WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); + if (!weakLink.expired()) { + SharedLinkInterfacePtr sharedLink = weakLink.lock(); + + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_message_t msg; + mavlink_msg_param_ext_request_list_pack_chan( + static_cast(mavlink->getSystemId()), + static_cast(mavlink->getComponentId()), + sharedLink->mavlinkChannel(), + &msg, + static_cast(_vehicle->id()), + static_cast(compID())); + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } + qCDebug(CameraControlVerboseLog) << "Request all parameters"; +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::_getParamName(const char* param_id) +{ + QByteArray bytes(param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + QString parameterName(bytes); + return parameterName; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleParamAck(const mavlink_param_ext_ack_t& ack) +{ + QString paramName = _getParamName(ack.param_id); + if(!_paramIO.contains(paramName)) { + qCWarning(CameraControlLog) << "Received PARAM_EXT_ACK for unknown param:" << paramName; + return; + } + if(_paramIO[paramName]) { + _paramIO[paramName]->handleParamAck(ack); + } else { + qCritical() << "QGCParamIO is NULL" << paramName; + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleParamValue(const mavlink_param_ext_value_t& value) +{ + QString paramName = _getParamName(value.param_id); + if(!_paramIO.contains(paramName)) { + qCWarning(CameraControlLog) << "Received PARAM_EXT_VALUE for unknown param:" << paramName; + return; + } + if(_paramIO[paramName]) { + _paramIO[paramName]->handleParamValue(value); + } else { + qCritical() << "QGCParamIO is NULL" << paramName; + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_updateActiveList() +{ + //-- Clear out excluded parameters based on exclusion rules + QStringList exclusionList; + for(QGCCameraOptionExclusion* param: _valueExclusions) { + Fact* pFact = getFact(param->param); + if(pFact) { + QString option = pFact->rawValueString(); + if(param->value == option) { + exclusionList << param->exclusions; + } + } + } + QStringList active; + for(QString key: _settings) { + if(!exclusionList.contains(key)) { + active.append(key); + } + } + if(active != _activeSettings) { + qCDebug(CameraControlVerboseLog) << "Excluding" << exclusionList; + _activeSettings = active; + emit activeSettingsChanged(); + //-- Force validity of "Facts" based on active set + if(_paramComplete) { + emit parametersReady(); + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_processConditionTest(const QString conditionTest) +{ + enum { + TEST_NONE, + TEST_EQUAL, + TEST_NOT_EQUAL, + TEST_GREATER, + TEST_SMALLER + }; + qCDebug(CameraControlVerboseLog) << "_processConditionTest(" << conditionTest << ")"; + int op = TEST_NONE; + QStringList test; + + auto split = [&conditionTest](const QString& sep ) { + return conditionTest.split(sep, Qt::SkipEmptyParts); + }; + + if(conditionTest.contains("!=")) { + test = split("!="); + op = TEST_NOT_EQUAL; + } else if(conditionTest.contains("=")) { + test = split("="); + op = TEST_EQUAL; + } else if(conditionTest.contains(">")) { + test = split(">"); + op = TEST_GREATER; + } else if(conditionTest.contains("<")) { + test = split("<"); + op = TEST_SMALLER; + } + if(test.size() == 2) { + Fact* pFact = getFact(test[0]); + if(pFact) { + switch(op) { + case TEST_EQUAL: + return pFact->rawValueString() == test[1]; + case TEST_NOT_EQUAL: + return pFact->rawValueString() != test[1]; + case TEST_GREATER: + return pFact->rawValueString() > test[1]; + case TEST_SMALLER: + return pFact->rawValueString() < test[1]; + case TEST_NONE: + break; + } + } else { + qWarning() << "Invalid condition parameter:" << test[0] << "in" << conditionTest; + return false; + } + } + qWarning() << "Invalid condition" << conditionTest; + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_processCondition(const QString condition) +{ + qCDebug(CameraControlVerboseLog) << "_processCondition(" << condition << ")"; + bool result = true; + bool andOp = true; + if(!condition.isEmpty()) { + QStringList scond = condition.split(" ", Qt::SkipEmptyParts); + while(scond.size()) { + QString test = scond.first(); + scond.removeFirst(); + if(andOp) { + result = result && _processConditionTest(test); + } else { + result = result || _processConditionTest(test); + } + if(!scond.size()) { + return result; + } + andOp = scond.first().toUpper() == "AND"; + scond.removeFirst(); + } + } + return result; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_updateRanges(Fact* pFact) +{ + QMap rangesSet; + QMap rangesReset; + QStringList changedList; + QStringList resetList; + QStringList updates; + //-- Iterate range sets looking for limited ranges + for(QGCCameraOptionRange* pRange: _optionRanges) { + //-- If this fact or one of its conditions is part of this range set + if(!changedList.contains(pRange->targetParam) && (pRange->param == pFact->name() || pRange->condition.contains(pFact->name()))) { + Fact* pRFact = getFact(pRange->param); //-- This parameter + Fact* pTFact = getFact(pRange->targetParam); //-- The target parameter (the one its range is to change) + if(pRFact && pTFact) { + //qCDebug(CameraControlVerboseLog) << "Check new set of options for" << pTFact->name(); + QString option = pRFact->rawValueString(); //-- This parameter value + //-- If this value (and condition) triggers a change in the target range + //qCDebug(CameraControlVerboseLog) << "Range value:" << pRange->value << "Current value:" << option << "Condition:" << pRange->condition; + if(pRange->value == option && _processCondition(pRange->condition)) { + if(pTFact->enumStrings() != pRange->optNames) { + //-- Set limited range set + rangesSet[pTFact] = pRange; + } + changedList << pRange->targetParam; + } + } + } + } + //-- Iterate range sets again looking for resets + for(QGCCameraOptionRange* pRange: _optionRanges) { + if(!changedList.contains(pRange->targetParam) && (pRange->param == pFact->name() || pRange->condition.contains(pFact->name()))) { + Fact* pTFact = getFact(pRange->targetParam); //-- The target parameter (the one its range is to change) + if(!resetList.contains(pRange->targetParam)) { + if(pTFact->enumStrings() != _originalOptNames[pRange->targetParam]) { + //-- Restore full option set + rangesReset[pTFact] = pRange->targetParam; + } + resetList << pRange->targetParam; + } + } + } + //-- Update limited range set + for (Fact* f: rangesSet.keys()) { + f->setEnumInfo(rangesSet[f]->optNames, rangesSet[f]->optVariants); + if(!updates.contains(f->name())) { + _paramIO[f->name()]->optNames = rangesSet[f]->optNames; + _paramIO[f->name()]->optVariants = rangesSet[f]->optVariants; + emit f->enumsChanged(); + qCDebug(CameraControlVerboseLog) << "Limited set of options for:" << f->name() << rangesSet[f]->optNames;; + updates << f->name(); + } + } + //-- Restore full range set + for (Fact* f: rangesReset.keys()) { + f->setEnumInfo(_originalOptNames[rangesReset[f]], _originalOptValues[rangesReset[f]]); + if(!updates.contains(f->name())) { + _paramIO[f->name()]->optNames = _originalOptNames[rangesReset[f]]; + _paramIO[f->name()]->optVariants = _originalOptValues[rangesReset[f]]; + emit f->enumsChanged(); + qCDebug(CameraControlVerboseLog) << "Restore full set of options for:" << f->name() << _originalOptNames[f->name()]; + updates << f->name(); + } + } + //-- Parameter update requests + if(_requestUpdates.contains(pFact->name())) { + for(const QString& param: _requestUpdates[pFact->name()]) { + if(!_updatesToRequest.contains(param)) { + _updatesToRequest << param; + } + } + } + if(_updatesToRequest.size()) { + QTimer::singleShot(500, this, &QGCCameraControl::_requestParamUpdates); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestParamUpdates() +{ + for(const QString& param: _updatesToRequest) { + _paramIO[param]->paramRequest(); + } + _updatesToRequest.clear(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestCameraSettings() +{ + qCDebug(CameraControlLog) << "_requestCameraSettings()"; + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_CAMERA_SETTINGS, // command id + false, // showError + 1); // Do Request + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestStorageInfo() +{ + qCDebug(CameraControlLog) << "_requestStorageInfo()"; + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_STORAGE_INFORMATION, // command id + false, // showError + 0, // Storage ID (0 for all, 1 for first, 2 for second, etc.) + 1); // Do Request + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleSettings(const mavlink_camera_settings_t& settings) +{ + qCDebug(CameraControlLog) << "handleSettings() Mode:" << settings.mode_id; + _setCameraMode(static_cast(settings.mode_id)); + qreal z = static_cast(settings.zoomLevel); + qreal f = static_cast(settings.focusLevel); + if(std::isfinite(z) && z != _zoomLevel) { + _zoomLevel = z; + emit zoomLevelChanged(); + } + if(std::isfinite(f) && f != _focusLevel) { + _focusLevel = f; + emit focusLevelChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleStorageInfo(const mavlink_storage_information_t& st) +{ + qCDebug(CameraControlLog) << "handleStorageInfo:" << st.available_capacity << st.status << st.storage_count << st.storage_id << st.total_capacity << st.used_capacity; + if(st.status == STORAGE_STATUS_READY) { + uint32_t t = static_cast(st.total_capacity); + if(_storageTotal != t) { + _storageTotal = t; + emit storageTotalChanged(); + } + uint32_t a = static_cast(st.available_capacity); + if(_storageFree != a) { + _storageFree = a; + emit storageFreeChanged(); + } + } + if(_storageStatus != static_cast(st.status)) { + _storageStatus = static_cast(st.status); + emit storageStatusChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleBatteryStatus(const mavlink_battery_status_t& bs) +{ + qCDebug(CameraControlLog) << "handleBatteryStatus:" << bs.battery_remaining; + if(bs.battery_remaining >= 0 && _batteryRemaining != static_cast(bs.battery_remaining)) { + _batteryRemaining = static_cast(bs.battery_remaining); + emit batteryRemainingChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleCaptureStatus(const mavlink_camera_capture_status_t& cap) +{ + //-- This is a response to MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS + qCDebug(CameraControlLog) << "handleCaptureStatus:" << cap.available_capacity << cap.image_interval << cap.image_status << cap.recording_time_ms << cap.video_status; + //-- Disk Free Space + uint32_t a = static_cast(cap.available_capacity); + if(_storageFree != a) { + _storageFree = a; + emit storageFreeChanged(); + } + //-- Do we have recording time? + if(cap.recording_time_ms) { + // Resync our _recTime timer to the time info received from the camera component + _recordTime = cap.recording_time_ms; + _recTime = _recTime.addMSecs(_recTime.msecsTo(QTime::currentTime()) - static_cast(cap.recording_time_ms)); + emit recordTimeChanged(); + } + //-- Video/Image Capture Status + uint8_t vs = cap.video_status < static_cast(VIDEO_CAPTURE_STATUS_LAST) ? cap.video_status : static_cast(VIDEO_CAPTURE_STATUS_UNDEFINED); + uint8_t ps = cap.image_status < static_cast(PHOTO_CAPTURE_LAST) ? cap.image_status : static_cast(PHOTO_CAPTURE_STATUS_UNDEFINED); + _setVideoStatus(static_cast(vs)); + _setPhotoStatus(static_cast(ps)); + //-- Keep asking for it once in a while when recording + if(videoStatus() == VIDEO_CAPTURE_STATUS_RUNNING) { + _captureStatusTimer.start(5000); + //-- Same while (single) image capture is busy + } else if(photoStatus() != PHOTO_CAPTURE_IDLE && photoMode() == PHOTO_CAPTURE_SINGLE) { + _captureStatusTimer.start(1000); + } + //-- Time Lapse + if(photoStatus() == PHOTO_CAPTURE_INTERVAL_IDLE || photoStatus() == PHOTO_CAPTURE_INTERVAL_IN_PROGRESS) { + //-- Capture local image as well + if(qgcApp()->toolbox()->videoManager()) { + QString photoPath = qgcApp()->toolbox()->settingsManager()->appSettings()->savePath()->rawValue().toString() + QStringLiteral("/Photo"); + QDir().mkpath(photoPath); + photoPath += + "/" + QDateTime::currentDateTime().toString("yyyy-MM-dd_hh.mm.ss.zzz") + ".jpg"; + qgcApp()->toolbox()->videoManager()->grabImage(photoPath); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleVideoInfo(const mavlink_video_stream_information_t* vi) +{ + qCDebug(CameraControlLog) << "handleVideoInfo:" << vi->stream_id << vi->uri; + _expectedCount = vi->count; + if(!_findStream(vi->stream_id, false)) { + qCDebug(CameraControlLog) << "Create stream handler for stream ID:" << vi->stream_id; + QGCVideoStreamInfo* pStream = new QGCVideoStreamInfo(this, vi); + QQmlEngine::setObjectOwnership(pStream, QQmlEngine::CppOwnership); + _streams.append(pStream); + //-- Thermal is handled separately and not listed + if(!pStream->isThermal()) { + _streamLabels.append(pStream->name()); + emit streamsChanged(); + emit streamLabelsChanged(); + } else { + emit thermalStreamChanged(); + } + } + //-- Check for missing count + if(_streams.count() < _expectedCount) { + _streamInfoTimer.start(1000); + } else { + //-- Done + qCDebug(CameraControlLog) << "All stream handlers done"; + _streamInfoTimer.stop(); + emit autoStreamChanged(); + emit _vehicle->cameraManager()->streamChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleVideoStatus(const mavlink_video_stream_status_t* vs) +{ + _streamStatusTimer.stop(); + qCDebug(CameraControlLog) << "handleVideoStatus:" << vs->stream_id; + QGCVideoStreamInfo* pInfo = _findStream(vs->stream_id); + if(pInfo) { + pInfo->update(vs); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setCurrentStream(int stream) +{ + if(stream != _currentStream && stream >= 0 && stream < _streamLabels.count()) { + if(_currentStream != stream) { + QGCVideoStreamInfo* pInfo = currentStreamInstance(); + if(pInfo) { + qCDebug(CameraControlLog) << "Stopping stream:" << pInfo->uri(); + //-- Stop current stream + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_STOP_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + } + _currentStream = stream; + pInfo = currentStreamInstance(); + if(pInfo) { + //-- Start new stream + qCDebug(CameraControlLog) << "Starting stream:" << pInfo->uri(); + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_START_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + //-- Update stream status + _requestStreamStatus(static_cast(pInfo->streamID())); + } + emit currentStreamChanged(); + emit _vehicle->cameraManager()->streamChanged(); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::stopStream() +{ + QGCVideoStreamInfo* pInfo = currentStreamInstance(); + if(pInfo) { + //-- Stop current stream + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_STOP_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::resumeStream() +{ + QGCVideoStreamInfo* pInfo = currentStreamInstance(); + if(pInfo) { + //-- Start new stream + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_VIDEO_START_STREAMING, // Command id + false, // ShowError + pInfo->streamID()); // Stream ID + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::autoStream() +{ + if(hasVideoStream()) { + return _streams.count() > 0; + } + return false; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::currentStreamInstance() +{ + if(_currentStream < _streamLabels.count() && _streamLabels.count()) { + QGCVideoStreamInfo* pStream = _findStream(_streamLabels[_currentStream]); + return pStream; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::thermalStreamInstance() +{ + //-- For now, it will return the first thermal listed (if any) + for(int i = 0; i < _streams.count(); i++) { + if(_streams[i]) { + QGCVideoStreamInfo* pStream = qobject_cast(_streams[i]); + if(pStream) { + if(pStream->isThermal()) { + return pStream; + } + } + } + } + return nullptr; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestStreamInfo(uint8_t streamID) +{ + qCDebug(CameraControlLog) << "Requesting video stream info for:" << streamID; + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION, // Command id + false, // ShowError + streamID); // Stream ID +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestStreamStatus(uint8_t streamID) +{ + qCDebug(CameraControlLog) << "Requesting video stream status for:" << streamID; + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_VIDEO_STREAM_STATUS, // Command id + false, // ShowError + streamID); // Stream ID + _streamStatusTimer.start(1000); // Wait up to a second for it +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::_findStream(uint8_t id, bool report) +{ + for(int i = 0; i < _streams.count(); i++) { + if(_streams[i]) { + QGCVideoStreamInfo* pStream = qobject_cast(_streams[i]); + if(pStream) { + if(pStream->streamID() == id) { + return pStream; + } + } else { + qCritical() << "Null QGCVideoStreamInfo instance"; + } + } + } + if(report) { + qWarning() << "Stream id not found:" << id; + } + return nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo* +QGCCameraControl::_findStream(const QString name) +{ + for(int i = 0; i < _streams.count(); i++) { + if(_streams[i]) { + QGCVideoStreamInfo* pStream = qobject_cast(_streams[i]); + if(pStream) { + if(pStream->name() == name) { + return pStream; + } + } + } + } + return nullptr; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_streamTimeout() +{ + _requestCount++; + int count = _expectedCount * 3; + if(_requestCount > count) { + qCWarning(CameraControlLog) << "Giving up requesting video stream info"; + _streamInfoTimer.stop(); + //-- If we have at least one stream, work with what we have. + if(_streams.count()) { + emit autoStreamChanged(); + emit _vehicle->cameraManager()->streamChanged(); + } + return; + } + for(uint8_t i = 0; i < _expectedCount; i++) { + //-- Stream ID starts at 1 + if(!_findStream(i+1, false)) { + _requestStreamInfo(i+1); + return; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_streamStatusTimeout() +{ + QGCVideoStreamInfo* pStream = currentStreamInstance(); + if(pStream) { + _requestStreamStatus(static_cast(pStream->streamID())); + } +} + +//----------------------------------------------------------------------------- +QStringList +QGCCameraControl::_loadExclusions(QDomNode option) +{ + QStringList exclusionList; + QDomElement optionElem = option.toElement(); + QDomNodeList excRoot = optionElem.elementsByTagName(kExclusions); + if(excRoot.size()) { + //-- Iterate exclusions + QDomNode node = excRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList exclusions = elem.elementsByTagName(kExclusion); + for(int i = 0; i < exclusions.size(); i++) { + QString exclude = exclusions.item(i).toElement().text(); + if(!exclude.isEmpty()) { + exclusionList << exclude; + } + } + } + return exclusionList; +} + +//----------------------------------------------------------------------------- +QStringList +QGCCameraControl::_loadUpdates(QDomNode option) +{ + QStringList updateList; + QDomElement optionElem = option.toElement(); + QDomNodeList updateRoot = optionElem.elementsByTagName(kUpdates); + if(updateRoot.size()) { + //-- Iterate updates + QDomNode node = updateRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList updates = elem.elementsByTagName(kUpdate); + for(int i = 0; i < updates.size(); i++) { + QString update = updates.item(i).toElement().text(); + if(!update.isEmpty()) { + updateList << update; + } + } + } + return updateList; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadRanges(QDomNode option, const QString factName, QString paramValue) +{ + QDomElement optionElem = option.toElement(); + QDomNodeList rangeRoot = optionElem.elementsByTagName(kParameterranges); + if(rangeRoot.size()) { + QDomNode node = rangeRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList parameterRanges = elem.elementsByTagName(kParameterrange); + //-- Iterate parameter ranges + for(int i = 0; i < parameterRanges.size(); i++) { + QString param; + QString condition; + QDomNode paramRange = parameterRanges.item(i); + if(!read_attribute(paramRange, kParameter, param)) { + qCritical() << QString("Malformed option range for parameter %1").arg(factName); + return false; + } + read_attribute(paramRange, kCondition, condition); + QDomElement pelem = paramRange.toElement(); + QDomNodeList rangeOptions = pelem.elementsByTagName(kRoption); + QStringList optNames; + QStringList optValues; + //-- Iterate options + for(int i = 0; i < rangeOptions.size(); i++) { + QString optName; + QString optValue; + QDomNode roption = rangeOptions.item(i); + if(!read_attribute(roption, kName, optName)) { + qCritical() << QString("Malformed roption for parameter %1").arg(factName); + return false; + } + if(!read_attribute(roption, kValue, optValue)) { + qCritical() << QString("Malformed rvalue for parameter %1").arg(factName); + return false; + } + optNames << optName; + optValues << optValue; + } + if(optNames.size()) { + QGCCameraOptionRange* pRange = new QGCCameraOptionRange(this, factName, paramValue, param, condition, optNames, optValues); + _optionRanges.append(pRange); + qCDebug(CameraControlVerboseLog) << "New range limit:" << factName << paramValue << param << condition << optNames << optValues; + } + } + } + return true; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_processRanges() +{ + //-- After all parameter are loaded, process parameter ranges + for(QGCCameraOptionRange* pRange: _optionRanges) { + Fact* pRFact = getFact(pRange->targetParam); + if(pRFact) { + for(int i = 0; i < pRange->optNames.size(); i++) { + QVariant optVariant; + QString errorString; + if (!pRFact->metaData()->convertAndValidateRaw(pRange->optValues[i], false, optVariant, errorString)) { + qWarning() << "Invalid roption value, name:" << pRange->targetParam + << " type:" << pRFact->metaData()->type() + << " value:" << pRange->optValues[i] + << " error:" << errorString; + } else { + pRange->optVariants << optVariant; + } + } + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadNameValue(QDomNode option, const QString factName, FactMetaData* metaData, QString& optName, QString& optValue, QVariant& optVariant) +{ + if(!read_attribute(option, kName, optName)) { + qCritical() << QString("Malformed option for parameter %1").arg(factName); + return false; + } + if(!read_attribute(option, kValue, optValue)) { + qCritical() << QString("Malformed value for parameter %1").arg(factName); + return false; + } + QString errorString; + if (!metaData->convertAndValidateRaw(optValue, false, optVariant, errorString)) { + qWarning() << "Invalid option value, name:" << factName + << " type:" << metaData->type() + << " value:" << optValue + << " error:" << errorString; + } + return true; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_handleDefinitionFile(const QString &url) +{ + //-- First check and see if we have it cached + QFile xmlFile(_cacheFile); + + QString ftpPrefix(QStringLiteral("%1://").arg(FTPManager::mavlinkFTPScheme)); + if (!xmlFile.exists() && url.startsWith(ftpPrefix, Qt::CaseInsensitive)) { + qCDebug(CameraControlLog) << "No camera definition file cached, attempt ftp download"; + int ver = static_cast(_info.cam_definition_version); + QString ext = ""; + if (url.endsWith(".lzma", Qt::CaseInsensitive)) { ext = ".lzma"; } + if (url.endsWith(".xz", Qt::CaseInsensitive)) { ext = ".xz"; } + QString fileName = QString::asprintf("%s_%s_%03d.xml%s", + _vendor.toStdString().c_str(), + _modelName.toStdString().c_str(), + ver, + ext.toStdString().c_str()); + connect(_vehicle->ftpManager(), &FTPManager::downloadComplete, this, &QGCCameraControl::_ftpDownloadComplete); + _vehicle->ftpManager()->download(_compID, url, + qgcApp()->toolbox()->settingsManager()->appSettings()->parameterSavePath().toStdString().c_str(), + fileName); + return; + } + + if (!xmlFile.exists()) { + qCDebug(CameraControlLog) << "No camera definition file cached, attempt http download"; + _httpRequest(url); + return; + } + if (!xmlFile.open(QIODevice::ReadOnly)) { + qWarning() << "Could not read cached camera definition file:" << _cacheFile; + _httpRequest(url); + return; + } + QByteArray bytes = xmlFile.readAll(); + QDomDocument doc; + if(!doc.setContent(bytes, false)) { + qWarning() << "Could not parse cached camera definition file:" << _cacheFile; + _httpRequest(url); + return; + } + //-- We have it + qCDebug(CameraControlLog) << "Using cached camera definition file:" << _cacheFile; + _cached = true; + emit dataReady(bytes); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_httpRequest(const QString &url) +{ + qCDebug(CameraControlLog) << "Request camera definition:" << url; + if(!_netManager) { + _netManager = new QNetworkAccessManager(this); + } + QNetworkProxy savedProxy = _netManager->proxy(); + QNetworkProxy tempProxy; + tempProxy.setType(QNetworkProxy::DefaultProxy); + _netManager->setProxy(tempProxy); + QNetworkRequest request(url); + request.setAttribute(QNetworkRequest::RedirectPolicyAttribute, true); + QSslConfiguration conf = request.sslConfiguration(); + conf.setPeerVerifyMode(QSslSocket::VerifyNone); + request.setSslConfiguration(conf); + QNetworkReply* reply = _netManager->get(request); + connect(reply, &QNetworkReply::finished, this, &QGCCameraControl::_downloadFinished); + _netManager->setProxy(savedProxy); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_downloadFinished() +{ + QNetworkReply* reply = qobject_cast(sender()); + if(!reply) { + return; + } + int err = reply->error(); + int http_code = reply->attribute(QNetworkRequest::HttpStatusCodeAttribute).toInt(); + QByteArray data = reply->readAll(); + if(err == QNetworkReply::NoError && http_code == 200) { + data.append("\n"); + } else { + data.clear(); + qWarning() << QString("Camera Definition (%1) download error: %2 status: %3").arg( + reply->url().toDisplayString(), + reply->errorString(), + reply->attribute(QNetworkRequest::HttpStatusCodeAttribute).toString() + ); + } + emit dataReady(data); + //reply->deleteLater(); +} + +void QGCCameraControl::_ftpDownloadComplete(const QString& fileName, const QString& errorMsg) +{ + qCDebug(CameraControlLog) << "FTP Download completed: " << fileName << ", " << errorMsg; + + disconnect(_vehicle->ftpManager(), &FTPManager::downloadComplete, this, &QGCCameraControl::_ftpDownloadComplete); + + QString outputFileName = fileName; + + if (fileName.endsWith(".lzma", Qt::CaseInsensitive) || fileName.endsWith(".xz", Qt::CaseInsensitive)) { + outputFileName = fileName.left(fileName.lastIndexOf(".")); + if (QGCLZMA::inflateLZMAFile(fileName, outputFileName)) { + QFile(fileName).remove(); + } else { + qCWarning(CameraControlLog) << "Inflate of compressed xml failed" << fileName; + outputFileName.clear(); + } + } + + QFile xmlFile(outputFileName); + + if (!xmlFile.exists()) { + qCDebug(CameraControlLog) << "No camera definition file present after ftp download completed"; + return; + } + if (!xmlFile.open(QIODevice::ReadOnly)) { + qWarning() << "Could not read downloaded camera definition file: " << fileName; + return; + } + + _cached = true; + QByteArray bytes = xmlFile.readAll(); + emit dataReady(bytes); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_dataReady(QByteArray data) +{ + if(data.size()) { + qCDebug(CameraControlLog) << "Parsing camera definition"; + _loadCameraDefinitionFile(data); + } else { + qCDebug(CameraControlLog) << "No camera definition"; + } + _initWhenReady(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_paramDone() +{ + for(const QString& param: _paramIO.keys()) { + if(!_paramIO[param]->paramDone()) { + return; + } + } + //-- All parameters loaded (or timed out) + _paramComplete = true; + emit parametersReady(); + //-- Check for video streaming + _checkForVideoStreams(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_checkForVideoStreams() +{ + if(_info.flags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM) { + //-- Skip it if using Taisync as it has its own video settings + if(!qgcApp()->toolbox()->videoManager()->isTaisync()) { + connect(&_streamInfoTimer, &QTimer::timeout, this, &QGCCameraControl::_streamTimeout); + _streamInfoTimer.setSingleShot(false); + connect(&_streamStatusTimer, &QTimer::timeout, this, &QGCCameraControl::_streamStatusTimeout); + _streamStatusTimer.setSingleShot(true); + //-- Request all streams + _requestStreamInfo(0); + _streamInfoTimer.start(2000); + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::incomingParameter(Fact* pFact, QVariant& newValue) +{ + Q_UNUSED(pFact); + Q_UNUSED(newValue); + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::validateParameter(Fact* pFact, QVariant& newValue) +{ + Q_UNUSED(pFact); + Q_UNUSED(newValue); + return true; +} + +//----------------------------------------------------------------------------- +QStringList +QGCCameraControl::activeSettings() +{ + qCDebug(CameraControlLog) << "Active:" << _activeSettings; + return _activeSettings; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::exposureMode() +{ + return (_paramComplete && _activeSettings.contains(kCAM_EXPMODE)) ? getFact(kCAM_EXPMODE) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::ev() +{ + return (_paramComplete && _activeSettings.contains(kCAM_EV)) ? getFact(kCAM_EV) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::iso() +{ + return (_paramComplete && _activeSettings.contains(kCAM_ISO)) ? getFact(kCAM_ISO) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::shutterSpeed() +{ + return (_paramComplete && _activeSettings.contains(kCAM_SHUTTERSPD)) ? getFact(kCAM_SHUTTERSPD) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::aperture() +{ + return (_paramComplete && _activeSettings.contains(kCAM_APERTURE)) ? getFact(kCAM_APERTURE) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::wb() +{ + return (_paramComplete && _activeSettings.contains(kCAM_WBMODE)) ? getFact(kCAM_WBMODE) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::mode() +{ + return _paramComplete && factExists(kCAM_MODE) ? getFact(kCAM_MODE) : nullptr; +} + +//----------------------------------------------------------------------------- +QGCVideoStreamInfo::QGCVideoStreamInfo(QObject* parent, const mavlink_video_stream_information_t *si) + : QObject(parent) +{ + memcpy(&_streamInfo, si, sizeof(mavlink_video_stream_information_t)); +} + +//----------------------------------------------------------------------------- +qreal +QGCVideoStreamInfo::aspectRatio() const +{ + qreal ar = 1.0; + if(_streamInfo.resolution_h && _streamInfo.resolution_v) { + ar = static_cast(_streamInfo.resolution_h) / static_cast(_streamInfo.resolution_v); + } + return ar; +} + +//----------------------------------------------------------------------------- +bool +QGCVideoStreamInfo::update(const mavlink_video_stream_status_t* vs) +{ + bool changed = false; + if(_streamInfo.hfov != vs->hfov) { + changed = true; + _streamInfo.hfov = vs->hfov; + } + if(_streamInfo.flags != vs->flags) { + changed = true; + _streamInfo.flags = vs->flags; + } + if(_streamInfo.bitrate != vs->bitrate) { + changed = true; + _streamInfo.bitrate = vs->bitrate; + } + if(_streamInfo.rotation != vs->rotation) { + changed = true; + _streamInfo.rotation = vs->rotation; + } + if(_streamInfo.framerate != vs->framerate) { + changed = true; + _streamInfo.framerate = vs->framerate; + } + if(_streamInfo.resolution_h != vs->resolution_h) { + changed = true; + _streamInfo.resolution_h = vs->resolution_h; + } + if(_streamInfo.resolution_v != vs->resolution_v) { + changed = true; + _streamInfo.resolution_v = vs->resolution_v; + } + if(changed) { + emit infoChanged(); + } + return changed; +} diff --git a/README.md b/src/README.md similarity index 100% rename from README.md rename to src/README.md diff --git a/src/api/QGCCorePlugin.cc b/src/api/QGCCorePlugin.cc new file mode 100644 index 0000000..318c7a9 --- /dev/null +++ b/src/api/QGCCorePlugin.cc @@ -0,0 +1,502 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QGCApplication.h" +#include "QGCCorePlugin.h" +#include "QGCOptions.h" +#include "QmlComponentInfo.h" +#include "FactMetaData.h" +#include "SettingsManager.h" +#include "AppMessages.h" +#include "QmlObjectListModel.h" +#include "VideoManager.h" +#if defined(QGC_GST_STREAMING) +#include "GStreamer.h" +#include "VideoReceiver.h" +#endif +#include "QGCLoggingCategory.h" +#include "QGCCameraManager.h" +#include "HorizontalFactValueGrid.h" +#include "InstrumentValueData.h" + +#include +#include + +/// @file +/// @brief Core Plugin Interface for QGroundControl - Default Implementation +/// @author Gus Grubba + +class QGCCorePlugin_p +{ +public: + QGCCorePlugin_p() + { + } + + ~QGCCorePlugin_p() + { + if(pGeneral) + delete pGeneral; + if(pCommLinks) + delete pCommLinks; + if(pOfflineMaps) + delete pOfflineMaps; +#if defined(QGC_GST_TAISYNC_ENABLED) + if(pTaisync) + delete pTaisync; +#endif +#if defined(QGC_GST_MICROHARD_ENABLED) + if(pMicrohard) + delete pMicrohard; +#endif + if(pMAVLink) + delete pMAVLink; + if(pConsole) + delete pConsole; +#if defined(QT_DEBUG) + if(pMockLink) + delete pMockLink; + if(pDebug) + delete pDebug; + if(pQmlTest) + delete pQmlTest; +#endif + if(pRemoteID) + delete pRemoteID; + if(defaultOptions) + delete defaultOptions; + } + + QmlComponentInfo* pGeneral = nullptr; + QmlComponentInfo* pCommLinks = nullptr; + QmlComponentInfo* pOfflineMaps = nullptr; +#if defined(QGC_GST_TAISYNC_ENABLED) + QmlComponentInfo* pTaisync = nullptr; +#endif +#if defined(QGC_GST_MICROHARD_ENABLED) + QmlComponentInfo* pMicrohard = nullptr; +#endif + QmlComponentInfo* pMAVLink = nullptr; + QmlComponentInfo* pConsole = nullptr; + QmlComponentInfo* pHelp = nullptr; +#if defined(QT_DEBUG) + QmlComponentInfo* pMockLink = nullptr; + QmlComponentInfo* pDebug = nullptr; + QmlComponentInfo* pQmlTest = nullptr; +#endif + QmlComponentInfo* pRemoteID = nullptr; + + QGCOptions* defaultOptions = nullptr; + QVariantList settingsList; + QVariantList analyzeList; + + QmlObjectListModel _emptyCustomMapItems; +}; + +QGCCorePlugin::~QGCCorePlugin() +{ + if(_p) { + delete _p; + } +} + +QGCCorePlugin::QGCCorePlugin(QGCApplication *app, QGCToolbox* toolbox) + : QGCTool(app, toolbox) + , _showTouchAreas(false) + , _showAdvancedUI(true) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + _p = new QGCCorePlugin_p; +} + +void QGCCorePlugin::setToolbox(QGCToolbox *toolbox) +{ + QGCTool::setToolbox(toolbox); + + qmlRegisterUncreatableType ("QGroundControl", 1, 0, "QGCCorePlugin", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl", 1, 0, "QGCOptions", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl", 1, 0, "QGCFlyViewOptions", "Reference only"); +} + +QVariantList &QGCCorePlugin::settingsPages() +{ + if(!_p->pGeneral) { + _p->pGeneral = new QmlComponentInfo(tr("General"), + QUrl::fromUserInput("qrc:/qml/GeneralSettings.qml"), + QUrl::fromUserInput("qrc:/res/gear-white.svg")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pGeneral))); + _p->pCommLinks = new QmlComponentInfo(tr("Comm Links"), + QUrl::fromUserInput("qrc:/qml/LinkSettings.qml"), + QUrl::fromUserInput("qrc:/res/waves.svg")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pCommLinks))); + _p->pOfflineMaps = new QmlComponentInfo(tr("Offline Maps"), + QUrl::fromUserInput("qrc:/qml/OfflineMap.qml"), + QUrl::fromUserInput("qrc:/res/waves.svg")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pOfflineMaps))); +#if defined(QGC_GST_TAISYNC_ENABLED) + _p->pTaisync = new QmlComponentInfo(tr("Taisync"), + QUrl::fromUserInput("qrc:/qml/TaisyncSettings.qml"), + QUrl::fromUserInput("")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pTaisync))); +#endif +#if defined(QGC_GST_MICROHARD_ENABLED) + _p->pMicrohard = new QmlComponentInfo(tr("Microhard"), + QUrl::fromUserInput("qrc:/qml/MicrohardSettings.qml"), + QUrl::fromUserInput("")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pMicrohard))); +#endif + _p->pMAVLink = new QmlComponentInfo(tr("MAVLink"), + QUrl::fromUserInput("qrc:/qml/MavlinkSettings.qml"), + QUrl::fromUserInput("qrc:/res/waves.svg")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pMAVLink))); + _p->pRemoteID = new QmlComponentInfo(tr("Remote ID"), + QUrl::fromUserInput("qrc:/qml/RemoteIDSettings.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pRemoteID))); + _p->pConsole = new QmlComponentInfo(tr("Console"), + QUrl::fromUserInput("qrc:/qml/QGroundControl/Controls/AppMessages.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pConsole))); + _p->pHelp = new QmlComponentInfo(tr("Help"), + QUrl::fromUserInput("qrc:/qml/HelpSettings.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pHelp))); +#if defined(QT_DEBUG) + //-- These are always present on Debug builds + _p->pMockLink = new QmlComponentInfo(tr("Mock Link"), + QUrl::fromUserInput("qrc:/qml/MockLink.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pMockLink))); + _p->pDebug = new QmlComponentInfo(tr("Debug"), + QUrl::fromUserInput("qrc:/qml/DebugWindow.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pDebug))); + _p->pQmlTest = new QmlComponentInfo(tr("Palette Test"), + QUrl::fromUserInput("qrc:/qml/QmlTest.qml")); + _p->settingsList.append(QVariant::fromValue(reinterpret_cast(_p->pQmlTest))); +#endif + } + return _p->settingsList; +} + +QVariantList& QGCCorePlugin::analyzePages() +{ + if (!_p->analyzeList.count()) { + _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("Log Download"), QUrl::fromUserInput("qrc:/qml/LogDownloadPage.qml"), QUrl::fromUserInput("qrc:/qmlimages/LogDownloadIcon")))); +#if !defined(__mobile__) + _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("GeoTag Images"), QUrl::fromUserInput("qrc:/qml/GeoTagPage.qml"), QUrl::fromUserInput("qrc:/qmlimages/GeoTagIcon")))); +#endif + _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("MAVLink Console"), QUrl::fromUserInput("qrc:/qml/MavlinkConsolePage.qml"), QUrl::fromUserInput("qrc:/qmlimages/MavlinkConsoleIcon")))); +#if !defined(QGC_DISABLE_MAVLINK_INSPECTOR) + _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("MAVLink Inspector"),QUrl::fromUserInput("qrc:/qml/MAVLinkInspectorPage.qml"), QUrl::fromUserInput("qrc:/qmlimages/MAVLinkInspector")))); +#endif + _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("Vibration"), QUrl::fromUserInput("qrc:/qml/VibrationPage.qml"), QUrl::fromUserInput("qrc:/qmlimages/VibrationPageIcon")))); + } + return _p->analyzeList; +} + +int QGCCorePlugin::defaultSettings() +{ + return 0; +} + +QGCOptions* QGCCorePlugin::options() +{ + if (!_p->defaultOptions) { + _p->defaultOptions = new QGCOptions(this); + } + return _p->defaultOptions; +} + +bool QGCCorePlugin::overrideSettingsGroupVisibility(QString name) +{ + Q_UNUSED(name); + + // Always show all + return true; +} + +bool QGCCorePlugin::adjustSettingMetaData(const QString& settingsGroup, FactMetaData& metaData) +{ + if (settingsGroup == AppSettings::settingsGroup) { +#if !defined(QGC_ENABLE_PAIRING) + //-- If we don't support pairing, disable it. + if (metaData.name() == AppSettings::usePairingName) { + metaData.setRawDefaultValue(false); + //-- And hide the option + return false; + } +#endif + + //-- Default Palette + if (metaData.name() == AppSettings::indoorPaletteName) { + QVariant outdoorPalette; +#if defined (__mobile__) + outdoorPalette = 0; +#else + outdoorPalette = 1; +#endif + metaData.setRawDefaultValue(outdoorPalette); + return true; + } + +#if defined (__mobile__) + if (metaData.name() == AppSettings::telemetrySaveName) { + // Mobile devices have limited storage so don't turn on telemtry saving by default + metaData.setRawDefaultValue(false); + return true; + } +#endif + +#ifndef __android__ + if (metaData.name() == AppSettings::androidSaveToSDCardName) { + // This only shows on android builds + return false; + } +#endif + } + + return true; // Show setting in ui +} + +void QGCCorePlugin::setShowTouchAreas(bool show) +{ + if (show != _showTouchAreas) { + _showTouchAreas = show; + emit showTouchAreasChanged(show); + } +} + +void QGCCorePlugin::setShowAdvancedUI(bool show) +{ + if (show != _showAdvancedUI) { + _showAdvancedUI = show; + emit showAdvancedUIChanged(show); + } +} + +void QGCCorePlugin::paletteOverride(QString /*colorName*/, QGCPalette::PaletteColorInfo_t& /*colorInfo*/) +{ + +} + +QString QGCCorePlugin::showAdvancedUIMessage() const +{ + return tr("WARNING: You are about to enter Advanced Mode. " + "If used incorrectly, this may cause your vehicle to malfunction thus voiding your warranty. " + "You should do so only if instructed by customer support. " + "Are you sure you want to enable Advanced Mode?"); +} + +void QGCCorePlugin::factValueGridCreateDefaultSettings(const QString& defaultSettingsGroup) +{ + HorizontalFactValueGrid factValueGrid(defaultSettingsGroup); + + bool includeFWValues = factValueGrid.vehicleClass() == QGCMAVLink::VehicleClassFixedWing || factValueGrid.vehicleClass() == QGCMAVLink::VehicleClassVTOL || factValueGrid.vehicleClass() == QGCMAVLink::VehicleClassAirship; + + factValueGrid.setFontSize(FactValueGrid::LargeFontSize); + + factValueGrid.appendColumn(); + factValueGrid.appendColumn(); + factValueGrid.appendColumn(); + if (includeFWValues) { + factValueGrid.appendColumn(); + } + factValueGrid.appendRow(); + + int rowIndex = 0; + QmlObjectListModel* column = factValueGrid.columns()->value(0); + + InstrumentValueData* value = column->value(rowIndex++); + value->setFact("Vehicle", "AltitudeRelative"); + value->setIcon("arrow-thick-up.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(true); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "DistanceToHome"); + value->setIcon("bookmark copy 3.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(true); + + rowIndex = 0; + column = factValueGrid.columns()->value(1); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "ClimbRate"); + value->setIcon("arrow-simple-up.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(true); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "GroundSpeed"); + value->setIcon("arrow-simple-right.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(true); + + + if (includeFWValues) { + rowIndex = 0; + column = factValueGrid.columns()->value(2); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "AirSpeed"); + value->setText("AirSpd"); + value->setShowUnits(true); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "ThrottlePct"); + value->setText("Thr"); + value->setShowUnits(true); + } + + rowIndex = 0; + column = factValueGrid.columns()->value(includeFWValues ? 3 : 2); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "FlightTime"); + value->setIcon("timer.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(false); + + value = column->value(rowIndex++); + value->setFact("Vehicle", "FlightDistance"); + value->setIcon("travel-walk.svg"); + value->setText(value->fact()->shortDescription()); + value->setShowUnits(true); +} + +QQmlApplicationEngine* QGCCorePlugin::createQmlApplicationEngine(QObject* parent) +{ + QQmlApplicationEngine* qmlEngine = new QQmlApplicationEngine(parent); + qmlEngine->addImportPath("qrc:/qml"); + qmlEngine->rootContext()->setContextProperty("joystickManager", qgcApp()->toolbox()->joystickManager()); + qmlEngine->rootContext()->setContextProperty("debugMessageModel", AppMessages::getModel()); + return qmlEngine; +} + +void QGCCorePlugin::createRootWindow(QQmlApplicationEngine* qmlEngine) +{ + qmlEngine->load(QUrl(QStringLiteral("qrc:/qml/MainRootWindow.qml"))); +} + +bool QGCCorePlugin::mavlinkMessage(Vehicle* vehicle, LinkInterface* link, mavlink_message_t message) +{ + Q_UNUSED(vehicle); + Q_UNUSED(link); + Q_UNUSED(message); + + return true; +} + +QmlObjectListModel* QGCCorePlugin::customMapItems() +{ + return &_p->_emptyCustomMapItems; +} + +VideoManager* QGCCorePlugin::createVideoManager(QGCApplication *app, QGCToolbox *toolbox) +{ + return new VideoManager(app, toolbox); +} + +VideoReceiver* QGCCorePlugin::createVideoReceiver(QObject* parent) +{ +#if defined(QGC_GST_STREAMING) + return GStreamer::createVideoReceiver(parent); +#else + Q_UNUSED(parent) + return nullptr; +#endif +} + +void* QGCCorePlugin::createVideoSink(QObject* parent, QQuickItem* widget) +{ +#if defined(QGC_GST_STREAMING) + return GStreamer::createVideoSink(parent, widget); +#else + Q_UNUSED(parent) + Q_UNUSED(widget) + return nullptr; +#endif +} + +void QGCCorePlugin::releaseVideoSink(void* sink) +{ +#if defined(QGC_GST_STREAMING) + GStreamer::releaseVideoSink(sink); +#else + Q_UNUSED(sink) +#endif +} + +bool QGCCorePlugin::guidedActionsControllerLogging() const +{ + return GuidedActionsControllerLog().isDebugEnabled(); +} + +QString QGCCorePlugin::stableVersionCheckFileUrl() const +{ +#ifdef QGC_CUSTOM_BUILD + // Custom builds must override to turn on and provide their own location + return QString(); +#else + return QString("https://s3-us-west-2.amazonaws.com/qgroundcontrol/latest/QGC.version.txt"); +#endif +} + +const QVariantList& QGCCorePlugin::toolBarIndicators(void) +{ + //-- Default list of indicators for all vehicles. + if(_toolBarIndicatorList.size() == 0) { + _toolBarIndicatorList = QVariantList({ + QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GPSRTKIndicator.qml")), + }); + } + return _toolBarIndicatorList; +} + +QList QGCCorePlugin::firstRunPromptStdIds(void) +{ + QList rgStdIds = { unitsFirstRunPromptId, offlineVehicleFirstRunPromptId }; + return rgStdIds; +} + +QList QGCCorePlugin::firstRunPromptCustomIds(void) +{ + return QList(); +} + +QVariantList QGCCorePlugin::firstRunPromptsToShow(void) +{ + QList rgIdsToShow; + + rgIdsToShow.append(firstRunPromptStdIds()); + rgIdsToShow.append(firstRunPromptCustomIds()); + + QList rgAlreadyShownIds = AppSettings::firstRunPromptsIdsVariantToList(_toolbox->settingsManager()->appSettings()->firstRunPromptIdsShown()->rawValue()); + + for (int idToRemove: rgAlreadyShownIds) { + rgIdsToShow.removeOne(idToRemove); + } + + QVariantList rgVarIdsToShow; + for (int id: rgIdsToShow) { + rgVarIdsToShow.append(id); + } + + return rgVarIdsToShow; +} + +QString QGCCorePlugin::firstRunPromptResource(int id) +{ + switch (id) { + case unitsFirstRunPromptId: + return "/FirstRunPromptDialogs/UnitsFirstRunPrompt.qml"; + case offlineVehicleFirstRunPromptId: + return "/FirstRunPromptDialogs/OfflineVehicleFirstRunPrompt.qml"; + break; + } + + return QString(); +} diff --git a/src/api/QGCCorePlugin.h b/src/api/QGCCorePlugin.h new file mode 100644 index 0000000..791724f --- /dev/null +++ b/src/api/QGCCorePlugin.h @@ -0,0 +1,222 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "QGCToolbox.h" +#include "QGCPalette.h" +#include "QGCMAVLink.h" +#include "QmlObjectListModel.h" + +#include +#include + +/// @file +/// @brief Core Plugin Interface for QGroundControl +/// @author Gus Grubba + +class QGCApplication; +class QGCOptions; +class QGCSettings; +class QGCCorePlugin_p; +class FactMetaData; +class QGeoPositionInfoSource; +class QQmlApplicationEngine; +class Vehicle; +class LinkInterface; +class QmlObjectListModel; +class VideoReceiver; +class VideoSink; +class PlanMasterController; +class QGCCameraManager; +class QGCCameraControl; +class QQuickItem; +class InstrumentValueAreaController; + +#ifndef OPAQUE_PTR_QGCCorePlugin + #define OPAQUE_PTR_QGCCorePlugin + Q_DECLARE_OPAQUE_POINTER(QGCOptions*) +#endif + +class QGCCorePlugin : public QGCTool +{ + Q_OBJECT +public: + QGCCorePlugin(QGCApplication* app, QGCToolbox* toolbox); + ~QGCCorePlugin(); + + Q_PROPERTY(QVariantList settingsPages READ settingsPages NOTIFY settingsPagesChanged) + Q_PROPERTY(QVariantList analyzePages READ analyzePages NOTIFY analyzePagesChanged) + Q_PROPERTY(int defaultSettings READ defaultSettings CONSTANT) + Q_PROPERTY(QGCOptions* options READ options CONSTANT) + Q_PROPERTY(bool showTouchAreas READ showTouchAreas WRITE setShowTouchAreas NOTIFY showTouchAreasChanged) + Q_PROPERTY(bool showAdvancedUI READ showAdvancedUI WRITE setShowAdvancedUI NOTIFY showAdvancedUIChanged) + Q_PROPERTY(QString showAdvancedUIMessage READ showAdvancedUIMessage CONSTANT) + Q_PROPERTY(QString brandImageIndoor READ brandImageIndoor CONSTANT) + Q_PROPERTY(QString brandImageOutdoor READ brandImageOutdoor CONSTANT) + Q_PROPERTY(QmlObjectListModel* customMapItems READ customMapItems CONSTANT) + Q_PROPERTY(QVariantList toolBarIndicators READ toolBarIndicators NOTIFY toolBarIndicatorsChanged) + Q_PROPERTY(int unitsFirstRunPromptId MEMBER unitsFirstRunPromptId CONSTANT) + Q_PROPERTY(int offlineVehicleFirstRunPromptId MEMBER offlineVehicleFirstRunPromptId CONSTANT) + + Q_INVOKABLE bool guidedActionsControllerLogging() const; + + /// The list of settings under the Settings Menu + /// @return A list of QGCSettings + virtual QVariantList& settingsPages(); + + /// The list of pages/buttons under the Analyze Menu + /// @return A list of QmlPageInfo + virtual QVariantList& analyzePages(); + + /// The default settings panel to show + /// @return The settings index + virtual int defaultSettings(); + + /// Global options + /// @return An instance of QGCOptions + virtual QGCOptions* options(); + + /// Allows the core plugin to override the visibility for a settings group + /// @param name - SettingsGroup name + /// @return true: Show settings ui, false: Hide settings ui + virtual bool overrideSettingsGroupVisibility(QString name); + + /// Allows the core plugin to override the setting meta data before the setting fact is created. + /// @param settingsGroup - QSettings group which contains this item + /// @param metaData - MetaData for setting fact + /// @return true: Setting should be visible in ui, false: Setting should not be shown in ui + virtual bool adjustSettingMetaData(const QString& settingsGroup, FactMetaData& metaData); + + /// Return the resource file which contains the brand image for for Indoor theme. + virtual QString brandImageIndoor() const { return QString(); } + + /// Return the resource file which contains the brand image for for Outdoor theme. + virtual QString brandImageOutdoor() const { return QString(); } + + /// @return The message to show to the user when they a re prompted to confirm turning on advanced ui. + virtual QString showAdvancedUIMessage() const; + + /// @return An instance of an alternate position source (or NULL if not available) + virtual QGeoPositionInfoSource* createPositionSource(QObject* /*parent*/) { return nullptr; } + + /// Allows a plugin to override the specified color name from the palette + virtual void paletteOverride(QString colorName, QGCPalette::PaletteColorInfo_t& colorInfo); + + virtual void factValueGridCreateDefaultSettings(const QString& defaultSettingsGroup); + + /// Allows the plugin to override or get access to the QmlApplicationEngine to do things like add import + /// path or stuff things into the context prior to window creation. + virtual QQmlApplicationEngine* createQmlApplicationEngine(QObject* parent); + + /// Allows the plugin to override the creation of the root (native) window. + virtual void createRootWindow(QQmlApplicationEngine* qmlEngine); + + /// Allows the plugin to override the creation of VideoManager. + virtual VideoManager* createVideoManager(QGCApplication* app, QGCToolbox* toolbox); + /// Allows the plugin to override the creation of VideoReceiver. + virtual VideoReceiver* createVideoReceiver(QObject* parent); + /// Allows the plugin to override the creation of VideoSink. + virtual void* createVideoSink(QObject* parent, QQuickItem* widget); + /// Allows the plugin to override the release of VideoSink. + virtual void releaseVideoSink(void* sink); + + /// Allows the plugin to see all mavlink traffic to a vehicle + /// @return true: Allow vehicle to continue processing, false: Vehicle should not process message + virtual bool mavlinkMessage(Vehicle* vehicle, LinkInterface* link, mavlink_message_t message); + + /// Allows custom builds to add custom items to the FlightMap. Objects put into QmlObjectListModel should derive from QmlComponentInfo and set the url property. + virtual QmlObjectListModel* customMapItems(); + + /// Allows custom builds to add custom items to the plan file before the document is created. + virtual void preSaveToJson (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {} + /// Allows custom builds to add custom items to the plan file after the document is created. + virtual void postSaveToJson (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {} + + /// Allows custom builds to add custom items to the mission section of the plan file before the item is created. + virtual void preSaveToMissionJson (PlanMasterController* /*pController*/, QJsonObject& /*missionJson*/) {} + /// Allows custom builds to add custom items to the mission section of the plan file after the item is created. + virtual void postSaveToMissionJson (PlanMasterController* /*pController*/, QJsonObject& /*missionJson*/) {} + + /// Allows custom builds to load custom items from the plan file before the document is parsed. + virtual void preLoadFromJson (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {} + /// Allows custom builds to load custom items from the plan file after the document is parsed. + virtual void postLoadFromJson (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {} + + /// Returns the url to download the stable version check file. Return QString() to indicate no version check should be performed. + /// Default QGC mainline implemenentation returns QGC Stable file location. Default QGC custom build code returns QString(). + /// Custom builds can override to turn on and provide their own location. + /// The contents of this file should be a single line in the form: + /// v3.4.4 + /// This indicates the latest stable version number. + virtual QString stableVersionCheckFileUrl() const; + + /// Returns the user visible url to show user where to download new stable builds from. + /// Custom builds must override to provide their own location. + virtual QString stableDownloadLocation() const { return QString("qgroundcontrol.com"); } + + /// Returns the complex mission items to display in the Plan UI + /// @param complexMissionItemNames Default set of complex items + /// @return Complex items to be made available to user + virtual QStringList complexMissionItemNames(Vehicle* /*vehicle*/, const QStringList& complexMissionItemNames) { return complexMissionItemNames; } + + /// Returns the standard list of first run prompt ids for possible display. Actual display is based on the + /// current AppSettings::firstRunPromptIds value. The order of this list also determines the order the prompts + /// will be displayed in. + virtual QList firstRunPromptStdIds(void); + + /// Returns the custom build list of first run prompt ids for possible display. Actual display is based on the + /// current AppSettings::firstRunPromptIds value. The order of this list also determines the order the prompts + /// will be displayed in. + virtual QList firstRunPromptCustomIds(void); + + /// Returns the resource which contains the specified first run prompt for display + Q_INVOKABLE virtual QString firstRunPromptResource(int id); + + /// Returns the list of toolbar indicators which are not related to a vehicle + /// signals toolbarIndicatorsChanged + /// @return A list of QUrl with the indicators + virtual const QVariantList& toolBarIndicators(void); + + /// Returns the list of first run prompt ids which need to be displayed according to current settings + Q_INVOKABLE QVariantList firstRunPromptsToShow(void); + + bool showTouchAreas() const { return _showTouchAreas; } + bool showAdvancedUI() const { return _showAdvancedUI; } + void setShowTouchAreas(bool show); + void setShowAdvancedUI(bool show); + + // Override from QGCTool + void setToolbox (QGCToolbox* toolbox); + + // Standard first run prompt ids + static const int unitsFirstRunPromptId = 1; + static const int offlineVehicleFirstRunPromptId = 2; + + // Custom builds can start there first run prompt ids from here + static const int firstRunPromptIdsFirstCustomId = 10000; + +signals: + void settingsPagesChanged (); + void analyzePagesChanged (); + void showTouchAreasChanged (bool showTouchAreas); + void showAdvancedUIChanged (bool showAdvancedUI); + void toolBarIndicatorsChanged (); + +protected: + bool _showTouchAreas; + bool _showAdvancedUI; + Vehicle* _activeVehicle = nullptr; + QGCCameraManager* _cameraManager = nullptr; + QGCCameraControl* _currentCamera = nullptr; + QVariantList _toolBarIndicatorList; + +private: + QGCCorePlugin_p* _p; +}; diff --git a/src/api/QGCOptions.cc b/src/api/QGCOptions.cc new file mode 100644 index 0000000..2e2c05c --- /dev/null +++ b/src/api/QGCOptions.cc @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QGCOptions.h" + +#include + +/// @file +/// @brief Core Plugin Interface for QGroundControl - Application Options +/// @author Gus Grubba + +QGCOptions::QGCOptions(QObject* parent) + : QObject(parent) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); +} + +QColor QGCOptions::toolbarBackgroundLight() const +{ + return QColor(255,255,255); +} + +QColor QGCOptions::toolbarBackgroundDark() const +{ + return QColor(0,0,0); +} + +QGCFlyViewOptions* QGCOptions::flyViewOptions(void) +{ + if (!_defaultFlyViewOptions) { + _defaultFlyViewOptions = new QGCFlyViewOptions(this); + } + return _defaultFlyViewOptions; +} + +QGCFlyViewOptions::QGCFlyViewOptions(QGCOptions* options, QObject* parent) + : QObject (parent) + , _options (options) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); +} diff --git a/src/api/QGCOptions.h b/src/api/QGCOptions.h new file mode 100644 index 0000000..7b5926d --- /dev/null +++ b/src/api/QGCOptions.h @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +class QGCOptions; + +class QGCFlyViewOptions : public QObject +{ + Q_OBJECT +public: + QGCFlyViewOptions(QGCOptions* options, QObject* parent = nullptr); + + Q_PROPERTY(bool showMultiVehicleList READ showMultiVehicleList CONSTANT) + Q_PROPERTY(bool showInstrumentPanel READ showInstrumentPanel CONSTANT) + Q_PROPERTY(bool showMapScale READ showMapScale CONSTANT) + Q_PROPERTY(bool guidedBarShowEmergencyStop READ guidedBarShowEmergencyStop NOTIFY guidedBarShowEmergencyStopChanged) + Q_PROPERTY(bool guidedBarShowOrbit READ guidedBarShowOrbit NOTIFY guidedBarShowOrbitChanged) + Q_PROPERTY(bool guidedBarShowROI READ guidedBarShowROI NOTIFY guidedBarShowROIChanged) + +protected: + virtual bool showMultiVehicleList () const { return true; } + virtual bool showMapScale () const { return true; } + virtual bool showInstrumentPanel () const { return true; } + virtual bool guidedBarShowEmergencyStop () const { return true; } + virtual bool guidedBarShowOrbit () const { return true; } + virtual bool guidedBarShowROI () const { return true; } + + QGCOptions* _options; + +signals: + void guidedBarShowEmergencyStopChanged (bool show); + void guidedBarShowOrbitChanged (bool show); + void guidedBarShowROIChanged (bool show); +}; + +class QGCOptions : public QObject +{ + Q_OBJECT +public: + QGCOptions(QObject* parent = nullptr); + + Q_PROPERTY(bool combineSettingsAndSetup READ combineSettingsAndSetup CONSTANT) + Q_PROPERTY(double toolbarHeightMultiplier READ toolbarHeightMultiplier CONSTANT) + Q_PROPERTY(bool enablePlanViewSelector READ enablePlanViewSelector CONSTANT) + Q_PROPERTY(QUrl preFlightChecklistUrl READ preFlightChecklistUrl CONSTANT) + Q_PROPERTY(bool showSensorCalibrationCompass READ showSensorCalibrationCompass NOTIFY showSensorCalibrationCompassChanged) + Q_PROPERTY(bool showSensorCalibrationGyro READ showSensorCalibrationGyro NOTIFY showSensorCalibrationGyroChanged) + Q_PROPERTY(bool showSensorCalibrationAccel READ showSensorCalibrationAccel NOTIFY showSensorCalibrationAccelChanged) + Q_PROPERTY(bool showSensorCalibrationLevel READ showSensorCalibrationLevel NOTIFY showSensorCalibrationLevelChanged) + Q_PROPERTY(bool showSensorCalibrationAirspeed READ showSensorCalibrationAirspeed NOTIFY showSensorCalibrationAirspeedChanged) + Q_PROPERTY(bool sensorsHaveFixedOrientation READ sensorsHaveFixedOrientation CONSTANT) + Q_PROPERTY(bool wifiReliableForCalibration READ wifiReliableForCalibration CONSTANT) + Q_PROPERTY(bool showFirmwareUpgrade READ showFirmwareUpgrade NOTIFY showFirmwareUpgradeChanged) + Q_PROPERTY(QString firmwareUpgradeSingleURL READ firmwareUpgradeSingleURL CONSTANT) + Q_PROPERTY(bool missionWaypointsOnly READ missionWaypointsOnly NOTIFY missionWaypointsOnlyChanged) + Q_PROPERTY(bool multiVehicleEnabled READ multiVehicleEnabled NOTIFY multiVehicleEnabledChanged) + Q_PROPERTY(bool showOfflineMapExport READ showOfflineMapExport NOTIFY showOfflineMapExportChanged) + Q_PROPERTY(bool showOfflineMapImport READ showOfflineMapImport NOTIFY showOfflineMapImportChanged) + Q_PROPERTY(bool useMobileFileDialog READ useMobileFileDialog CONSTANT) + Q_PROPERTY(bool showMissionStatus READ showMissionStatus CONSTANT) + Q_PROPERTY(bool guidedActionsRequireRCRSSI READ guidedActionsRequireRCRSSI CONSTANT) + Q_PROPERTY(bool showMissionAbsoluteAltitude READ showMissionAbsoluteAltitude NOTIFY showMissionAbsoluteAltitudeChanged) + Q_PROPERTY(bool showSimpleMissionStart READ showSimpleMissionStart NOTIFY showSimpleMissionStartChanged) + Q_PROPERTY(bool disableVehicleConnection READ disableVehicleConnection CONSTANT) + Q_PROPERTY(float devicePixelRatio READ devicePixelRatio NOTIFY devicePixelRatioChanged) + Q_PROPERTY(float devicePixelDensity READ devicePixelDensity NOTIFY devicePixelDensityChanged) + Q_PROPERTY(bool checkFirmwareVersion READ checkFirmwareVersion CONSTANT) + Q_PROPERTY(bool showMavlinkLogOptions READ showMavlinkLogOptions CONSTANT) + Q_PROPERTY(bool enableSaveMainWindowPosition READ enableSaveMainWindowPosition CONSTANT) + Q_PROPERTY(QStringList surveyBuiltInPresetNames READ surveyBuiltInPresetNames CONSTANT) + Q_PROPERTY(bool allowJoystickSelection READ allowJoystickSelection NOTIFY allowJoystickSelectionChanged) + + Q_PROPERTY(QGCFlyViewOptions* flyView READ flyViewOptions CONSTANT) + + /// Should QGC hide its settings menu and colapse it into one single menu (Settings and Vehicle Setup)? + /// @return true if QGC should consolidate both menus into one. + virtual bool combineSettingsAndSetup () { return false; } + + /// Main ToolBar Multiplier. + /// @return Factor to use when computing toolbar height + virtual double toolbarHeightMultiplier () { return 1.0; } + + /// Enable Plan View Selector (Mission, Fence or Rally) + /// @return True or false + virtual bool enablePlanViewSelector () { return true; } + + /// Should the mission status indicator (Plan View) be shown? + /// @return Yes or no + virtual bool showMissionStatus () { return true; } + + /// Provides an optional, custom preflight checklist + virtual QUrl preFlightChecklistUrl () const { return QUrl::fromUserInput("qrc:/qml/PreFlightCheckList.qml"); } + + /// Allows replacing the toolbar Light Theme color + virtual QColor toolbarBackgroundLight () const; + /// Allows replacing the toolbar Dark Theme color + virtual QColor toolbarBackgroundDark () const; + /// By returning false you can hide the following sensor calibration pages + virtual bool showSensorCalibrationCompass () const { return true; } + virtual bool showSensorCalibrationGyro () const { return true; } + virtual bool showSensorCalibrationAccel () const { return true; } + virtual bool showSensorCalibrationLevel () const { return true; } + virtual bool showSensorCalibrationAirspeed () const { return true; } + virtual bool wifiReliableForCalibration () const { return false; } + virtual bool sensorsHaveFixedOrientation () const { return false; } + virtual bool showFirmwareUpgrade () const { return true; } + virtual bool missionWaypointsOnly () const { return false; } ///< true: Only allow waypoints and complex items in Plan + virtual bool multiVehicleEnabled () const { return true; } ///< false: multi vehicle support is disabled + virtual bool guidedActionsRequireRCRSSI () const { return false; } ///< true: Guided actions will be disabled is there is no RC RSSI + virtual bool showOfflineMapExport () const { return true; } + virtual bool showOfflineMapImport () const { return true; } + virtual bool showMissionAbsoluteAltitude () const { return true; } + virtual bool showSimpleMissionStart () const { return false; } + virtual bool disableVehicleConnection () const { return false; } ///< true: vehicle connection is disabled + virtual bool checkFirmwareVersion () const { return true; } + virtual bool showMavlinkLogOptions () const { return true; } + virtual bool allowJoystickSelection () const { return true; } ///< false: custom build has automatically enabled a specific joystick + /// Desktop builds save the main application size and position on close (and restore it on open) + virtual bool enableSaveMainWindowPosition () const { return true; } + virtual QStringList surveyBuiltInPresetNames () const { return QStringList(); } // Built in presets cannot be deleted + +#if defined(__mobile__) + virtual bool useMobileFileDialog () const { return true;} +#else + virtual bool useMobileFileDialog () const { return false;} +#endif + + /// If returned QString in non-empty it means that firmware upgrade will run in a mode which only + /// supports downloading a single firmware file from the URL. It also supports custom install through + /// the Advanced options. + virtual QString firmwareUpgradeSingleURL () const { return QString(); } + + /// Device specific pixel ratio/density (for when Qt doesn't properly read it from the hardware) + virtual float devicePixelRatio () const { return 0.0f; } + virtual float devicePixelDensity () const { return 0.0f; } + + virtual QGCFlyViewOptions* flyViewOptions (); + +signals: + void showSensorCalibrationCompassChanged (bool show); + void showSensorCalibrationGyroChanged (bool show); + void showSensorCalibrationAccelChanged (bool show); + void showSensorCalibrationLevelChanged (bool show); + void showSensorCalibrationAirspeedChanged (bool show); + void showFirmwareUpgradeChanged (bool show); + void missionWaypointsOnlyChanged (bool missionWaypointsOnly); + void multiVehicleEnabledChanged (bool multiVehicleEnabled); + void allowJoystickSelectionChanged (bool allow); + void showOfflineMapExportChanged (); + void showOfflineMapImportChanged (); + void showMissionAbsoluteAltitudeChanged (); + void showSimpleMissionStartChanged (); + void devicePixelRatioChanged (); + void devicePixelDensityChanged (); + +protected: + QGCFlyViewOptions* _defaultFlyViewOptions = nullptr; +}; diff --git a/src/api/QGCSettings.cc b/src/api/QGCSettings.cc new file mode 100644 index 0000000..c296d64 --- /dev/null +++ b/src/api/QGCSettings.cc @@ -0,0 +1,21 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QGCSettings.h" + +/// @file +/// @brief Core Plugin Interface for QGroundControl. Settings element. +/// @author Gus Grubba + +QGCSettings::QGCSettings(QString title, QUrl url, QUrl icon) + : _title(title) + , _url(url) + , _icon(icon) +{ +} diff --git a/src/api/QGCSettings.h b/src/api/QGCSettings.h new file mode 100644 index 0000000..593a178 --- /dev/null +++ b/src/api/QGCSettings.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +/// @file +/// @brief Core Plugin Interface for QGroundControl. Settings element. +/// @author Gus Grubba + +class QGCSettings : public QObject +{ + Q_OBJECT +public: + QGCSettings(QString title, QUrl url, QUrl icon = QUrl()); + + Q_PROPERTY(QString title READ title CONSTANT) + Q_PROPERTY(QUrl url READ url CONSTANT) + Q_PROPERTY(QUrl icon READ icon CONSTANT) + + virtual QString title () { return _title; } + virtual QUrl url () { return _url; } + virtual QUrl icon () { return _icon; } + +protected: + QString _title; + QUrl _url; + QUrl _icon; +}; diff --git a/src/api/QmlComponentInfo.cc b/src/api/QmlComponentInfo.cc new file mode 100644 index 0000000..35bb33c --- /dev/null +++ b/src/api/QmlComponentInfo.cc @@ -0,0 +1,19 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QmlComponentInfo.h" + +QmlComponentInfo::QmlComponentInfo(QString title, QUrl url, QUrl icon, QObject* parent) + : QObject (parent) + , _title (title) + , _url (url) + , _icon (icon) +{ + +} diff --git a/src/api/QmlComponentInfo.h b/src/api/QmlComponentInfo.h new file mode 100644 index 0000000..30c0459 --- /dev/null +++ b/src/api/QmlComponentInfo.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +/// Represents a Qml component which can be loaded from a resource. +class QmlComponentInfo : public QObject +{ + Q_OBJECT + +public: + QmlComponentInfo(QString title, QUrl url, QUrl icon = QUrl(), QObject* parent = nullptr); + + Q_PROPERTY(QString title READ title CONSTANT) ///< Title for page + Q_PROPERTY(QUrl url READ url CONSTANT) ///< Qml source code + Q_PROPERTY(QUrl icon READ icon CONSTANT) ///< Icon for page + + virtual QString title () { return _title; } + virtual QUrl url () { return _url; } + virtual QUrl icon () { return _icon; } + +protected: + QString _title; + QUrl _url; + QUrl _icon; +}; From 4083b4a6f74c6fbf56dd9505a18d5ca4aa895bec Mon Sep 17 00:00:00 2001 From: laptoy <2107884590@qq.com> Date: Tue, 11 Jun 2024 15:02:01 +0800 Subject: [PATCH 8/9] add --- src/{QGCCameraControl.cc => QGCCameraControl.c} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/{QGCCameraControl.cc => QGCCameraControl.c} (99%) diff --git a/src/QGCCameraControl.cc b/src/QGCCameraControl.c similarity index 99% rename from src/QGCCameraControl.cc rename to src/QGCCameraControl.c index f85acb7..3d28a7a 100644 --- a/src/QGCCameraControl.cc +++ b/src/QGCCameraControl.c @@ -2230,7 +2230,7 @@ QGCVideoStreamInfo::update(const mavlink_video_stream_status_t* vs) } if(_streamInfo.flags != vs->flags) { changed = true; - _streamInfo.flags = vs->flags; + _streamInfo.flags = vs->flags;// } if(_streamInfo.bitrate != vs->bitrate) { changed = true; From 3b0f1f871867883d4175328fcfc726bc7bfd32e1 Mon Sep 17 00:00:00 2001 From: laptoy <2107884590@qq.com> Date: Tue, 11 Jun 2024 15:07:22 +0800 Subject: [PATCH 9/9] add --- src/Joystick/CMakeLists.txt | 40 + src/Joystick/Joystick.cc | 1171 ++++++++++++++++++++++ src/Joystick/Joystick.h | 370 +++++++ src/Joystick/JoystickAndroid.cc | 293 ++++++ src/Joystick/JoystickAndroid.h | 54 + src/Joystick/JoystickManager.cc | 226 +++++ src/Joystick/JoystickManager.h | 82 ++ src/Joystick/JoystickMavCommand.cc | 101 ++ src/Joystick/JoystickMavCommand.h | 40 + src/Joystick/JoystickMavCommands.json | 17 + src/Joystick/JoystickSDL.cc | 178 ++++ src/Joystick/JoystickSDL.h | 53 + src/PositionManager/CMakeLists.txt | 16 + src/PositionManager/PositionManager.cpp | 202 ++++ src/PositionManager/PositionManager.h | 74 ++ src/PositionManager/SimulatedPosition.cc | 96 ++ src/PositionManager/SimulatedPosition.h | 49 + 17 files changed, 3062 insertions(+) create mode 100644 src/Joystick/CMakeLists.txt create mode 100644 src/Joystick/Joystick.cc create mode 100644 src/Joystick/Joystick.h create mode 100644 src/Joystick/JoystickAndroid.cc create mode 100644 src/Joystick/JoystickAndroid.h create mode 100644 src/Joystick/JoystickManager.cc create mode 100644 src/Joystick/JoystickManager.h create mode 100644 src/Joystick/JoystickMavCommand.cc create mode 100644 src/Joystick/JoystickMavCommand.h create mode 100644 src/Joystick/JoystickMavCommands.json create mode 100644 src/Joystick/JoystickSDL.cc create mode 100644 src/Joystick/JoystickSDL.h create mode 100644 src/PositionManager/CMakeLists.txt create mode 100644 src/PositionManager/PositionManager.cpp create mode 100644 src/PositionManager/PositionManager.h create mode 100644 src/PositionManager/SimulatedPosition.cc create mode 100644 src/PositionManager/SimulatedPosition.h diff --git a/src/Joystick/CMakeLists.txt b/src/Joystick/CMakeLists.txt new file mode 100644 index 0000000..83b1065 --- /dev/null +++ b/src/Joystick/CMakeLists.txt @@ -0,0 +1,40 @@ + +set(EXTRA_SRC) + +if (ANDROID) + list(APPEND EXTRA_SRC + JoystickAndroid.cc + ) +endif() + +add_library(Joystick + Joystick.cc + JoystickManager.cc + JoystickSDL.cc + JoystickMavCommand.cc + ${EXTRA_SRC} +) + +target_link_libraries(Joystick + PRIVATE + ui + PUBLIC + qgc + ui +) + +target_include_directories(Joystick PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +if(WIN32) + target_link_libraries(Joystick PUBLIC sdl2) +else() + find_package(SDL2 REQUIRED) + if (IS_DIRECTORY "${SDL2_INCLUDE_DIRS}") + include_directories(${SDL2_INCLUDE_DIRS}) + string(STRIP "${SDL2_LIBRARIES}" SDL2_LIBRARIES) + target_link_libraries(Joystick PRIVATE ${SDL2_LIBRARIES}) + else() + include_directories(${SDL2_DIR}) + target_link_libraries(Joystick PRIVATE SDL2::SDL2) + endif() +endif() diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc new file mode 100644 index 0000000..b9835c4 --- /dev/null +++ b/src/Joystick/Joystick.cc @@ -0,0 +1,1171 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +#include "Joystick.h" +#include "QGC.h" +#include "AutoPilotPlugin.h" +#include "UAS.h" +#include "QGCApplication.h" +#include "VideoManager.h" +#include "QGCCameraManager.h" +#include "QGCCameraControl.h" + +#include + +// JoystickLog Category declaration moved to QGCLoggingCategory.cc to allow access in Vehicle +QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog") + +const char* Joystick::_settingsGroup = "Joysticks"; +const char* Joystick::_calibratedSettingsKey = "Calibrated4"; // Increment number to force recalibration +const char* Joystick::_buttonActionNameKey = "ButtonActionName%1"; +const char* Joystick::_buttonActionRepeatKey = "ButtonActionRepeat%1"; +const char* Joystick::_throttleModeSettingsKey = "ThrottleMode"; +const char* Joystick::_negativeThrustSettingsKey = "NegativeThrust"; +const char* Joystick::_exponentialSettingsKey = "Exponential"; +const char* Joystick::_accumulatorSettingsKey = "Accumulator"; +const char* Joystick::_deadbandSettingsKey = "Deadband"; +const char* Joystick::_circleCorrectionSettingsKey = "Circle_Correction"; +const char* Joystick::_axisFrequencySettingsKey = "AxisFrequency"; +const char* Joystick::_buttonFrequencySettingsKey = "ButtonFrequency"; +const char* Joystick::_txModeSettingsKey = nullptr; +const char* Joystick::_fixedWingTXModeSettingsKey = "TXMode_FixedWing"; +const char* Joystick::_multiRotorTXModeSettingsKey = "TXMode_MultiRotor"; +const char* Joystick::_roverTXModeSettingsKey = "TXMode_Rover"; +const char* Joystick::_vtolTXModeSettingsKey = "TXMode_VTOL"; +const char* Joystick::_submarineTXModeSettingsKey = "TXMode_Submarine"; + +const char* Joystick::_buttonActionNone = QT_TR_NOOP("No Action"); +const char* Joystick::_buttonActionArm = QT_TR_NOOP("Arm"); +const char* Joystick::_buttonActionDisarm = QT_TR_NOOP("Disarm"); +const char* Joystick::_buttonActionToggleArm = QT_TR_NOOP("Toggle Arm"); +const char* Joystick::_buttonActionVTOLFixedWing = QT_TR_NOOP("VTOL: Fixed Wing"); +const char* Joystick::_buttonActionVTOLMultiRotor = QT_TR_NOOP("VTOL: Multi-Rotor"); +const char* Joystick::_buttonActionContinuousZoomIn = QT_TR_NOOP("Continuous Zoom In"); +const char* Joystick::_buttonActionContinuousZoomOut = QT_TR_NOOP("Continuous Zoom Out"); +const char* Joystick::_buttonActionStepZoomIn = QT_TR_NOOP("Step Zoom In"); +const char* Joystick::_buttonActionStepZoomOut = QT_TR_NOOP("Step Zoom Out"); +const char* Joystick::_buttonActionNextStream = QT_TR_NOOP("Next Video Stream"); +const char* Joystick::_buttonActionPreviousStream = QT_TR_NOOP("Previous Video Stream"); +const char* Joystick::_buttonActionNextCamera = QT_TR_NOOP("Next Camera"); +const char* Joystick::_buttonActionPreviousCamera = QT_TR_NOOP("Previous Camera"); +const char* Joystick::_buttonActionTriggerCamera = QT_TR_NOOP("Trigger Camera"); +const char* Joystick::_buttonActionStartVideoRecord = QT_TR_NOOP("Start Recording Video"); +const char* Joystick::_buttonActionStopVideoRecord = QT_TR_NOOP("Stop Recording Video"); +const char* Joystick::_buttonActionToggleVideoRecord = QT_TR_NOOP("Toggle Recording Video"); +const char* Joystick::_buttonActionGimbalDown = QT_TR_NOOP("Gimbal Down"); +const char* Joystick::_buttonActionGimbalUp = QT_TR_NOOP("Gimbal Up"); +const char* Joystick::_buttonActionGimbalLeft = QT_TR_NOOP("Gimbal Left"); +const char* Joystick::_buttonActionGimbalRight = QT_TR_NOOP("Gimbal Right"); +const char* Joystick::_buttonActionGimbalCenter = QT_TR_NOOP("Gimbal Center"); +const char* Joystick::_buttonActionEmergencyStop = QT_TR_NOOP("Emergency Stop"); +const char* Joystick::_buttonActionGripperGrab = QT_TR_NOOP("Gripper Close"); +const char* Joystick::_buttonActionGripperRelease = QT_TR_NOOP("Gripper Open"); + +const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { + "RollAxis", + "PitchAxis", + "YawAxis", + "ThrottleAxis", + "GimbalPitchAxis", + "GimbalYawAxis" +}; + +int Joystick::_transmitterMode = 2; + +const float Joystick::_defaultAxisFrequencyHz = 25.0f; +const float Joystick::_defaultButtonFrequencyHz = 5.0f; +const float Joystick::_minAxisFrequencyHz = 0.25f; +const float Joystick::_maxAxisFrequencyHz = 200.0f; +const float Joystick::_minButtonFrequencyHz = 0.25f; +const float Joystick::_maxButtonFrequencyHz = 50.0f; + +AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString name) + : QObject(parent) + , action(name) +{ +} + +AssignableButtonAction::AssignableButtonAction(QObject* parent, QString action_, bool canRepeat_) + : QObject(parent) + , _action(action_) + , _repeat(canRepeat_) +{ +} + +Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager) + : _name(name) + , _axisCount(axisCount) + , _buttonCount(buttonCount) + , _hatCount(hatCount) + , _hatButtonCount(4 * hatCount) + , _totalButtonCount(_buttonCount+_hatButtonCount) + , _multiVehicleManager(multiVehicleManager) +{ + qRegisterMetaType(); + + _rgAxisValues = new int[static_cast(_axisCount)]; + _rgCalibration = new Calibration_t[static_cast(_axisCount)]; + _rgButtonValues = new uint8_t[static_cast(_totalButtonCount)]; + for (int i = 0; i < _axisCount; i++) { + _rgAxisValues[i] = 0; + } + for (int i = 0; i < _totalButtonCount; i++) { + _rgButtonValues[i] = BUTTON_UP; + _buttonActionArray.append(nullptr); + } + _buildActionList(_multiVehicleManager->activeVehicle()); + _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle()); + _loadSettings(); + connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged); + connect(qgcApp()->toolbox()->multiVehicleManager()->vehicles(), &QmlObjectListModel::countChanged, this, &Joystick::_vehicleCountChanged); + + _customMavCommands = JoystickMavCommand::load("JoystickMavCommands.json"); +} + +void Joystick::stop() +{ + _exitThread = true; + wait(); +} + +Joystick::~Joystick() +{ + if (!_exitThread) { + qWarning() << "Joystick thread still running!"; + } + delete[] _rgAxisValues; + delete[] _rgCalibration; + delete[] _rgButtonValues; + _assignableButtonActions.clearAndDeleteContents(); + for (int button = 0; button < _totalButtonCount; button++) { + if(_buttonActionArray[button]) { + _buttonActionArray[button]->deleteLater(); + } + } +} + +void Joystick::_setDefaultCalibration(void) { + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + _calibrated = settings.value(_calibratedSettingsKey, false).toBool(); + + // Only set default calibrations if we do not have a calibration for this gamecontroller + if(_calibrated) return; + + for(int axis = 0; axis < _axisCount; axis++) { + Joystick::Calibration_t calibration; + _rgCalibration[axis] = calibration; + } + + _rgCalibration[1].reversed = true; + _rgCalibration[3].reversed = true; + + // Default TX Mode 2 axis assignments for gamecontrollers + _rgFunctionAxis[rollFunction] = 2; + _rgFunctionAxis[pitchFunction] = 3; + _rgFunctionAxis[yawFunction] = 0; + _rgFunctionAxis[throttleFunction] = 1; + + _rgFunctionAxis[gimbalPitchFunction]= 4; + _rgFunctionAxis[gimbalYawFunction] = 5; + + _exponential = 0; + _accumulator = false; + _deadband = false; + _axisFrequencyHz = _defaultAxisFrequencyHz; + _buttonFrequencyHz = _defaultButtonFrequencyHz; + _throttleMode = ThrottleModeDownZero; + _calibrated = true; + _circleCorrection = false; + + _saveSettings(); +} + +void Joystick::_updateTXModeSettingsKey(Vehicle* activeVehicle) +{ + if(activeVehicle) { + if(activeVehicle->fixedWing()) { + _txModeSettingsKey = _fixedWingTXModeSettingsKey; + } else if(activeVehicle->multiRotor()) { + _txModeSettingsKey = _multiRotorTXModeSettingsKey; + } else if(activeVehicle->rover()) { + _txModeSettingsKey = _roverTXModeSettingsKey; + } else if(activeVehicle->vtol()) { + _txModeSettingsKey = _vtolTXModeSettingsKey; + } else if(activeVehicle->sub()) { + _txModeSettingsKey = _submarineTXModeSettingsKey; + } else { + _txModeSettingsKey = nullptr; + qWarning() << "No valid joystick TXmode settings key for selected vehicle"; + return; + } + } else { + _txModeSettingsKey = nullptr; + } +} + +void Joystick::_activeVehicleChanged(Vehicle* activeVehicle) +{ + _updateTXModeSettingsKey(activeVehicle); + if(activeVehicle) { + QSettings settings; + settings.beginGroup(_settingsGroup); + int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt(); + setTXMode(mode); + } +} +void Joystick::_flightModesChanged() +{ + _buildActionList(_activeVehicle); +} + +void Joystick::_vehicleCountChanged(int count) +{ + if(count == 0) + { + // then the last vehicle has been deleted + qCDebug(JoystickLog) << "Stopping joystick thread due to last active vehicle deletion"; + this->stopPolling(); + } +} + +void Joystick::_loadSettings() +{ + QSettings settings; + settings.beginGroup(_settingsGroup); + Vehicle* activeVehicle = _multiVehicleManager->activeVehicle(); + + if(_txModeSettingsKey && activeVehicle) + _transmitterMode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt(); + + settings.beginGroup(_name); + + bool badSettings = false; + bool convertOk; + + qCDebug(JoystickLog) << "_loadSettings " << _name; + + _calibrated = settings.value(_calibratedSettingsKey, false).toBool(); + _exponential = settings.value(_exponentialSettingsKey, 0).toFloat(); + _accumulator = settings.value(_accumulatorSettingsKey, false).toBool(); + _deadband = settings.value(_deadbandSettingsKey, false).toBool(); + _axisFrequencyHz = settings.value(_axisFrequencySettingsKey, _defaultAxisFrequencyHz).toFloat(); + _buttonFrequencyHz = settings.value(_buttonFrequencySettingsKey, _defaultButtonFrequencyHz).toFloat(); + _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool(); + _negativeThrust = settings.value(_negativeThrustSettingsKey, false).toBool(); + + + _throttleMode = static_cast(settings.value(_throttleModeSettingsKey, ThrottleModeDownZero).toInt(&convertOk)); + badSettings |= !convertOk; + + qCDebug(JoystickLog) << "_loadSettings calibrated:txmode:throttlemode:exponential:deadband:badsettings" << _calibrated << _transmitterMode << _throttleMode << _exponential << _deadband << badSettings; + + QString minTpl ("Axis%1Min"); + QString maxTpl ("Axis%1Max"); + QString trimTpl ("Axis%1Trim"); + QString revTpl ("Axis%1Rev"); + QString deadbndTpl ("Axis%1Deadbnd"); + + for (int axis = 0; axis < _axisCount; axis++) { + Calibration_t* calibration = &_rgCalibration[axis]; + + calibration->center = settings.value(trimTpl.arg(axis), 0).toInt(&convertOk); + badSettings |= !convertOk; + + calibration->min = settings.value(minTpl.arg(axis), -32768).toInt(&convertOk); + badSettings |= !convertOk; + + calibration->max = settings.value(maxTpl.arg(axis), 32767).toInt(&convertOk); + badSettings |= !convertOk; + + calibration->deadband = settings.value(deadbndTpl.arg(axis), 0).toInt(&convertOk); + badSettings |= !convertOk; + + calibration->reversed = settings.value(revTpl.arg(axis), false).toBool(); + + qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:deadband:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings; + } + + int workingAxis = 0; + for (int function = 0; function < maxFunction; function++) { + int functionAxis; + functionAxis = settings.value(_rgFunctionSettingsKey[function], -1).toInt(&convertOk); + badSettings |= !convertOk || (functionAxis >= _axisCount); + if(functionAxis >= 0) { + workingAxis++; + } + if(functionAxis < _axisCount) { + _rgFunctionAxis[function] = functionAxis; + } + qCDebug(JoystickLog) << "_loadSettings function:axis:badsettings" << function << functionAxis << badSettings; + } + badSettings |= workingAxis < 4; + + // FunctionAxis mappings are always stored in TX mode 2 + // Remap to stored TX mode in settings + _remapAxes(2, _transmitterMode, _rgFunctionAxis); + + for (int button = 0; button < _totalButtonCount; button++) { + QString a = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString(); + if(!a.isEmpty() && a != _buttonActionNone) { + if(_buttonActionArray[button]) { + _buttonActionArray[button]->deleteLater(); + } + AssignedButtonAction* ap = new AssignedButtonAction(this, a); + ap->repeat = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool(); + _buttonActionArray[button] = ap; + _buttonActionArray[button]->buttonTime.start(); + qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; + } + } + + if (badSettings) { + _calibrated = false; + settings.setValue(_calibratedSettingsKey, false); + } +} + +void Joystick::_saveButtonSettings() +{ + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + for (int button = 0; button < _totalButtonCount; button++) { + if(_buttonActionArray[button]) { + settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); + qCDebug(JoystickLog) << "_saveButtonSettings button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; + } + } +} + +void Joystick::_saveSettings() +{ + QSettings settings; + settings.beginGroup(_settingsGroup); + + // Transmitter mode is static + // Save the mode we are using + if(_txModeSettingsKey) + settings.setValue(_txModeSettingsKey, _transmitterMode); + + settings.beginGroup(_name); + settings.setValue(_calibratedSettingsKey, _calibrated); + settings.setValue(_exponentialSettingsKey, _exponential); + settings.setValue(_accumulatorSettingsKey, _accumulator); + settings.setValue(_deadbandSettingsKey, _deadband); + settings.setValue(_axisFrequencySettingsKey, _axisFrequencyHz); + settings.setValue(_buttonFrequencySettingsKey, _buttonFrequencyHz); + settings.setValue(_throttleModeSettingsKey, _throttleMode); + settings.setValue(_negativeThrustSettingsKey, _negativeThrust); + settings.setValue(_circleCorrectionSettingsKey, _circleCorrection); + + qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode; + + QString minTpl ("Axis%1Min"); + QString maxTpl ("Axis%1Max"); + QString trimTpl ("Axis%1Trim"); + QString revTpl ("Axis%1Rev"); + QString deadbndTpl ("Axis%1Deadbnd"); + + for (int axis = 0; axis < _axisCount; axis++) { + Calibration_t* calibration = &_rgCalibration[axis]; + settings.setValue(trimTpl.arg(axis), calibration->center); + settings.setValue(minTpl.arg(axis), calibration->min); + settings.setValue(maxTpl.arg(axis), calibration->max); + settings.setValue(revTpl.arg(axis), calibration->reversed); + settings.setValue(deadbndTpl.arg(axis), calibration->deadband); + qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed:deadband" + << _name + << axis + << calibration->min + << calibration->max + << calibration->center + << calibration->reversed + << calibration->deadband; + } + + // Always save function Axis mappings in TX Mode 2 + // Write mode 2 mappings without changing mapping currently in use + int temp[maxFunction]; + _remapAxes(_transmitterMode, 2, temp); + for (int function = 0; function < maxFunction; function++) { + settings.setValue(_rgFunctionSettingsKey[function], temp[function]); + qCDebug(JoystickLog) << "_saveSettings name:function:axis" << _name << function << _rgFunctionSettingsKey[function]; + } + _saveButtonSettings(); +} + +// Relative mappings of axis functions between different TX modes +int Joystick::_mapFunctionMode(int mode, int function) { + static const int mapping[][6] = { + { yawFunction, pitchFunction, rollFunction, throttleFunction, gimbalPitchFunction, gimbalYawFunction }, + { yawFunction, throttleFunction, rollFunction, pitchFunction, gimbalPitchFunction, gimbalYawFunction }, + { rollFunction, pitchFunction, yawFunction, throttleFunction, gimbalPitchFunction, gimbalYawFunction }, + { rollFunction, throttleFunction, yawFunction, pitchFunction, gimbalPitchFunction, gimbalYawFunction }}; + return mapping[mode-1][function]; +} + +// Remap current axis functions from current TX mode to new TX mode +void Joystick::_remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]) { + int temp[maxFunction]; + for(int function = 0; function < maxFunction; function++) { + temp[_mapFunctionMode(newMode, function)] = _rgFunctionAxis[_mapFunctionMode(currentMode, function)]; + } + for(int function = 0; function < maxFunction; function++) { + newMapping[function] = temp[function]; + } +} + +void Joystick::setTXMode(int mode) { + if(mode > 0 && mode <= 4) { + _remapAxes(_transmitterMode, mode, _rgFunctionAxis); + _transmitterMode = mode; + _saveSettings(); + } else { + qCWarning(JoystickLog) << "Invalid mode:" << mode; + } +} + +/// Adjust the raw axis value to the -1:1 range given calibration information +float Joystick::_adjustRange(int value, Calibration_t calibration, bool withDeadbands) +{ + float valueNormalized; + float axisLength; + float axisBasis; + + if (value > calibration.center) { + axisBasis = 1.0f; + valueNormalized = value - calibration.center; + axisLength = calibration.max - calibration.center; + } else { + axisBasis = -1.0f; + valueNormalized = calibration.center - value; + axisLength = calibration.center - calibration.min; + } + + float axisPercent; + + if (withDeadbands) { + if (valueNormalized>calibration.deadband) { + axisPercent = (valueNormalized - calibration.deadband) / (axisLength - calibration.deadband); + } else if (valueNormalized<-calibration.deadband) { + axisPercent = (valueNormalized + calibration.deadband) / (axisLength - calibration.deadband); + } else { + axisPercent = 0.f; + } + } + else { + axisPercent = valueNormalized / axisLength; + } + + float correctedValue = axisBasis * axisPercent; + + if (calibration.reversed) { + correctedValue *= -1.0f; + } + +#if 0 + qCDebug(JoystickLog) << "_adjustRange corrected:value:min:max:center:reversed:deadband:basis:normalized:length" + << correctedValue + << value + << calibration.min + << calibration.max + << calibration.center + << calibration.reversed + << calibration.deadband + << axisBasis + << valueNormalized + << axisLength; +#endif + + return std::max(-1.0f, std::min(correctedValue, 1.0f)); +} + + +void Joystick::run() +{ + //-- Joystick thread + _open(); + //-- Reset timers + _axisTime.start(); + for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { + if(_buttonActionArray[buttonIndex]) { + _buttonActionArray[buttonIndex]->buttonTime.start(); + } + } + while (!_exitThread) { + _update(); + _handleButtons(); + if (axisCount() != 0) { + _handleAxis(); + } + QGC::SLEEP::msleep(qMin(static_cast(1000.0f / _maxAxisFrequencyHz), static_cast(1000.0f / _maxButtonFrequencyHz)) / 2); + } + _close(); +} + +void Joystick::_handleButtons() +{ + int lastBbuttonValues[256]; + //-- Update button states + for (int buttonIndex = 0; buttonIndex < _buttonCount; buttonIndex++) { + bool newButtonValue = _getButton(buttonIndex); + if(buttonIndex < 256) + lastBbuttonValues[buttonIndex] = _rgButtonValues[buttonIndex]; + if (newButtonValue && _rgButtonValues[buttonIndex] == BUTTON_UP) { + _rgButtonValues[buttonIndex] = BUTTON_DOWN; + emit rawButtonPressedChanged(buttonIndex, newButtonValue); + } else if (!newButtonValue && _rgButtonValues[buttonIndex] != BUTTON_UP) { + _rgButtonValues[buttonIndex] = BUTTON_UP; + emit rawButtonPressedChanged(buttonIndex, newButtonValue); + } + } + //-- Update hat - append hat buttons to the end of the normal button list + int numHatButtons = 4; + for (int hatIndex = 0; hatIndex < _hatCount; hatIndex++) { + for (int hatButtonIndex = 0; hatButtonIndexaction; + if(buttonAction.isEmpty() || buttonAction == _buttonActionNone) + continue; + if(!_buttonActionArray[buttonIndex]->repeat) { + //-- This button just went down + if(_rgButtonValues[buttonIndex] == BUTTON_DOWN) { + // Check for a multi-button action + QList rgButtons = { buttonIndex }; + bool executeButtonAction = true; + for (int multiIndex = 0; multiIndex < _totalButtonCount; multiIndex++) { + if (multiIndex != buttonIndex) { + if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action == buttonAction) { + // We found a multi-button action + if (_rgButtonValues[multiIndex] == BUTTON_DOWN || _rgButtonValues[multiIndex] == BUTTON_REPEAT) { + // So far so good + rgButtons.append(multiIndex); + continue; + } else { + // We are missing a press we need + executeButtonAction = false; + break; + } + } + } + } + if (executeButtonAction) { + qCDebug(JoystickLog) << "Action triggered" << rgButtons << buttonAction; + _executeButtonAction(buttonAction, true); + } + } + } else { + //-- Process repeat buttons + int buttonDelay = static_cast(1000.0f / _buttonFrequencyHz); + if(_buttonActionArray[buttonIndex]->buttonTime.elapsed() > buttonDelay) { + _buttonActionArray[buttonIndex]->buttonTime.start(); + qCDebug(JoystickLog) << "Repeat button triggered" << buttonIndex << buttonAction; + _executeButtonAction(buttonAction, true); + } + } + } + //-- Flag it as processed + _rgButtonValues[buttonIndex] = BUTTON_REPEAT; + } else if(_rgButtonValues[buttonIndex] == BUTTON_UP) { + //-- Button up transition + if(buttonIndex < 256) { + if(lastBbuttonValues[buttonIndex] == BUTTON_DOWN || lastBbuttonValues[buttonIndex] == BUTTON_REPEAT) { + if(_buttonActionArray[buttonIndex]) { + QString buttonAction = _buttonActionArray[buttonIndex]->action; + if(buttonAction.isEmpty() || buttonAction == _buttonActionNone) + continue; + qCDebug(JoystickLog) << "Button up" << buttonIndex << buttonAction; + _executeButtonAction(buttonAction, false); + } + } + } + } + } +} + +void Joystick::_handleAxis() +{ + //-- Get frequency + int axisDelay = static_cast(1000.0f / _axisFrequencyHz); + //-- Check elapsed time since last run + if(_axisTime.elapsed() > axisDelay) { + _axisTime.start(); + //-- Update axis + for (int axisIndex = 0; axisIndex < _axisCount; axisIndex++) { + int newAxisValue = _getAxis(axisIndex); + // Calibration code requires signal to be emitted even if value hasn't changed + _rgAxisValues[axisIndex] = newAxisValue; + emit rawAxisValueChanged(axisIndex, newAxisValue); + } + if (_activeVehicle->joystickEnabled() && !_calibrationMode && _calibrated) { + int axis = _rgFunctionAxis[rollFunction]; + float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + + axis = _rgFunctionAxis[pitchFunction]; + float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + + axis = _rgFunctionAxis[yawFunction]; + float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + + axis = _rgFunctionAxis[throttleFunction]; + float throttle = _adjustRange(_rgAxisValues[axis],_rgCalibration[axis], _throttleMode==ThrottleModeDownZero?false:_deadband); + + float gimbalPitch = 0.0f; + float gimbalYaw = 0.0f; + + if(_axisCount > 4) { + axis = _rgFunctionAxis[gimbalPitchFunction]; + gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + } + + if(_axisCount > 5) { + axis = _rgFunctionAxis[gimbalYawFunction]; + gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + } + + if (_accumulator) { + static float throttle_accu = 0.f; + throttle_accu += throttle * (40 / 1000.f); //for throttle to change from min to max it will take 1000ms (40ms is a loop time) + throttle_accu = std::max(static_cast(-1.f), std::min(throttle_accu, static_cast(1.f))); + throttle = throttle_accu; + } + + if (_circleCorrection) { + float roll_limited = std::max(static_cast(-M_PI_4), std::min(roll, static_cast(M_PI_4))); + float pitch_limited = std::max(static_cast(-M_PI_4), std::min(pitch, static_cast(M_PI_4))); + float yaw_limited = std::max(static_cast(-M_PI_4), std::min(yaw, static_cast(M_PI_4))); + float throttle_limited = std::max(static_cast(-M_PI_4), std::min(throttle, static_cast(M_PI_4))); + + // Map from unit circle to linear range and limit + roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f)); + pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f)); + yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f)); + throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f)); + } + + if ( _exponential < -0.01f) { + // Exponential (0% to -50% range like most RC radios) + // _exponential is set by a slider in joystickConfigAdvanced.qml + // Calculate new RPY with exponential applied + roll = -_exponential*powf(roll, 3) + (1+_exponential)*roll; + pitch = -_exponential*powf(pitch,3) + (1+_exponential)*pitch; + yaw = -_exponential*powf(yaw, 3) + (1+_exponential)*yaw; + } + + // Adjust throttle to 0:1 range + if (_throttleMode == ThrottleModeCenterZero && _activeVehicle->supportsThrottleModeCenterZero()) { + if (!_activeVehicle->supportsNegativeThrust() || !_negativeThrust) { + throttle = std::max(0.0f, throttle); + } + } else { + throttle = (throttle + 1.0f) / 2.0f; + } + qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle:gimbalPitch:gimbalYaw" << name() << roll << -pitch << yaw << throttle << gimbalPitch << gimbalYaw; + // NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub (and it only handles 16 bits) + // Set up button bitmap + quint64 buttonPressedBits = 0; // Buttons pressed for manualControl signal + for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { + quint64 buttonBit = static_cast(1LL << buttonIndex); + if (_rgButtonValues[buttonIndex] != BUTTON_UP) { + // Mark the button as pressed as long as its pressed + buttonPressedBits |= buttonBit; + } + } + emit axisValues(roll, pitch, yaw, throttle); + + uint16_t shortButtons = static_cast(buttonPressedBits & 0xFFFF); + _activeVehicle->sendJoystickDataThreadSafe(roll, pitch, yaw, throttle, shortButtons); + } + } +} + +void Joystick::startPolling(Vehicle* vehicle) +{ + if (vehicle) { + // If a vehicle is connected, disconnect it + if (_activeVehicle) { + disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); + disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); + disconnect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); + disconnect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); + disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue); + disconnect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); + disconnect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); + disconnect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); + } + // Always set up the new vehicle + _activeVehicle = vehicle; + // If joystick is not calibrated, disable it + if ( axisCount() != 0 && !_calibrated ) { + vehicle->setJoystickEnabled(false); + } + // Update qml in case of joystick transition + emit calibratedChanged(_calibrated); + // Build action list + _buildActionList(vehicle); + // Only connect the new vehicle if it wants joystick data + if (vehicle->joystickEnabled()) { + _pollingStartedForCalibration = false; + connect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); + connect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + connect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + connect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); + connect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); + connect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); + connect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue); + connect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); + connect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); + connect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); + } + } + if (!isRunning()) { + _exitThread = false; + start(); + } +} + +void Joystick::stopPolling(void) +{ + if (isRunning()) { + if (_activeVehicle && _activeVehicle->joystickEnabled()) { + disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); + disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); + disconnect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); + disconnect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); + disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue); + disconnect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); + disconnect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); + } + _exitThread = true; + } +} + +void Joystick::setCalibration(int axis, Calibration_t& calibration) +{ + if (!_validAxis(axis)) { + return; + } + _calibrated = true; + _rgCalibration[axis] = calibration; + _saveSettings(); + emit calibratedChanged(_calibrated); +} + +Joystick::Calibration_t Joystick::getCalibration(int axis) +{ + if (!_validAxis(axis)) { + return Calibration_t(); + } + return _rgCalibration[axis]; +} + +void Joystick::setFunctionAxis(AxisFunction_t function, int axis) +{ + if (!_validAxis(axis)) { + return; + } + _calibrated = true; + _rgFunctionAxis[function] = axis; + _saveSettings(); + emit calibratedChanged(_calibrated); +} + +int Joystick::getFunctionAxis(AxisFunction_t function) +{ + if (static_cast(function) < 0 || function >= maxFunction) { + qCWarning(JoystickLog) << "Invalid function" << function; + } + return _rgFunctionAxis[function]; +} + +void Joystick::setButtonRepeat(int button, bool repeat) +{ + if (!_validButton(button) || !_buttonActionArray[button]) { + return; + } + _buttonActionArray[button]->repeat = repeat; + _buttonActionArray[button]->buttonTime.start(); + //-- Save to settings + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); +} + +bool Joystick::getButtonRepeat(int button) +{ + if (!_validButton(button) || !_buttonActionArray[button]) { + return false; + } + return _buttonActionArray[button]->repeat; +} + +void Joystick::setButtonAction(int button, const QString& action) +{ + if (!_validButton(button)) { + return; + } + qCWarning(JoystickLog) << "setButtonAction:" << button << action; + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + if(action.isEmpty() || action == _buttonActionNone) { + if(_buttonActionArray[button]) { + _buttonActionArray[button]->deleteLater(); + _buttonActionArray[button] = nullptr; + //-- Clear from settings + settings.remove(QString(_buttonActionNameKey).arg(button)); + settings.remove(QString(_buttonActionRepeatKey).arg(button)); + } + } else { + if(!_buttonActionArray[button]) { + _buttonActionArray[button] = new AssignedButtonAction(this, action); + } else { + _buttonActionArray[button]->action = action; + //-- Make sure repeat is off if this action doesn't support repeats + int idx = _findAssignableButtonAction(action); + if(idx >= 0) { + AssignableButtonAction* p = qobject_cast(_assignableButtonActions[idx]); + if(!p->canRepeat()) { + _buttonActionArray[button]->repeat = false; + } + } + } + //-- Save to settings + settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); + } + emit buttonActionsChanged(); +} + +QString Joystick::getButtonAction(int button) +{ + if (_validButton(button)) { + if(_buttonActionArray[button]) { + return _buttonActionArray[button]->action; + } + } + return QString(_buttonActionNone); +} + +QStringList Joystick::buttonActions() +{ + QStringList list; + for (int button = 0; button < _totalButtonCount; button++) { + list << getButtonAction(button); + } + return list; +} + +int Joystick::throttleMode() +{ + return _throttleMode; +} + +void Joystick::setThrottleMode(int mode) +{ + if (mode < 0 || mode >= ThrottleModeMax) { + qCWarning(JoystickLog) << "Invalid throttle mode" << mode; + return; + } + _throttleMode = static_cast(mode); + if (_throttleMode == ThrottleModeDownZero) { + setAccumulator(false); + } + _saveSettings(); + emit throttleModeChanged(_throttleMode); +} + +bool Joystick::negativeThrust() const +{ + return _negativeThrust; +} + +void Joystick::setNegativeThrust(bool allowNegative) +{ + if (_negativeThrust == allowNegative) { + return; + } + _negativeThrust = allowNegative; + _saveSettings(); + emit negativeThrustChanged(_negativeThrust); +} + +float Joystick::exponential() const +{ + return _exponential; +} + +void Joystick::setExponential(float expo) +{ + _exponential = expo; + _saveSettings(); + emit exponentialChanged(_exponential); +} + +bool Joystick::accumulator() const +{ + return _accumulator; +} + +void Joystick::setAccumulator(bool accu) +{ + _accumulator = accu; + _saveSettings(); + emit accumulatorChanged(_accumulator); +} + +bool Joystick::deadband() const +{ + return _deadband; +} + +void Joystick::setDeadband(bool deadband) +{ + _deadband = deadband; + _saveSettings(); +} + +bool Joystick::circleCorrection() const +{ + return _circleCorrection; +} + +void Joystick::setCircleCorrection(bool circleCorrection) +{ + _circleCorrection = circleCorrection; + _saveSettings(); + emit circleCorrectionChanged(_circleCorrection); +} + +void Joystick::setAxisFrequency(float val) +{ + //-- Arbitrary limits + val = qMax(_minAxisFrequencyHz, val); + val = qMin(_maxAxisFrequencyHz, val); + _axisFrequencyHz = val; + _saveSettings(); + emit axisFrequencyHzChanged(); +} + +void Joystick::setButtonFrequency(float val) +{ + //-- Arbitrary limits + val = qMax(_minButtonFrequencyHz, val); + val = qMin(_maxButtonFrequencyHz, val); + _buttonFrequencyHz = val; + _saveSettings(); + emit buttonFrequencyHzChanged(); +} + +void Joystick::setCalibrationMode(bool calibrating) +{ + _calibrationMode = calibrating; + if (calibrating && !isRunning()) { + _pollingStartedForCalibration = true; + startPolling(_multiVehicleManager->activeVehicle()); + } + else if (_pollingStartedForCalibration) { + stopPolling(); + } +} + + +void Joystick::_executeButtonAction(const QString& action, bool buttonDown) +{ + if (!_activeVehicle || !_activeVehicle->joystickEnabled() || action == _buttonActionNone) { + return; + } + if (action == _buttonActionArm) { + if (buttonDown) emit setArmed(true); + } else if (action == _buttonActionDisarm) { + if (buttonDown) emit setArmed(false); + } else if (action == _buttonActionToggleArm) { + if (buttonDown) emit setArmed(!_activeVehicle->armed()); + } else if (action == _buttonActionVTOLFixedWing) { + if (buttonDown) emit setVtolInFwdFlight(true); + } else if (action == _buttonActionVTOLMultiRotor) { + if (buttonDown) emit setVtolInFwdFlight(false); + } else if (_activeVehicle->flightModes().contains(action)) { + if (buttonDown) emit setFlightMode(action); + } else if(action == _buttonActionContinuousZoomIn || action == _buttonActionContinuousZoomOut) { + if (buttonDown) { + emit startContinuousZoom(action == _buttonActionContinuousZoomIn ? 1 : -1); + } else { + emit stopContinuousZoom(); + } + } else if(action == _buttonActionStepZoomIn || action == _buttonActionStepZoomOut) { + if (buttonDown) emit stepZoom(action == _buttonActionStepZoomIn ? 1 : -1); + } else if(action == _buttonActionNextStream || action == _buttonActionPreviousStream) { + if (buttonDown) emit stepStream(action == _buttonActionNextStream ? 1 : -1); + } else if(action == _buttonActionNextCamera || action == _buttonActionPreviousCamera) { + if (buttonDown) emit stepCamera(action == _buttonActionNextCamera ? 1 : -1); + } else if(action == _buttonActionTriggerCamera) { + if (buttonDown) emit triggerCamera(); + } else if(action == _buttonActionStartVideoRecord) { + if (buttonDown) emit startVideoRecord(); + } else if(action == _buttonActionStopVideoRecord) { + if (buttonDown) emit stopVideoRecord(); + } else if(action == _buttonActionToggleVideoRecord) { + if (buttonDown) emit toggleVideoRecord(); + } else if(action == _buttonActionGimbalUp) { + if (buttonDown) _pitchStep(1); + } else if(action == _buttonActionGimbalDown) { + if (buttonDown) _pitchStep(-1); + } else if(action == _buttonActionGimbalLeft) { + if (buttonDown) _yawStep(-1); + } else if(action == _buttonActionGimbalRight) { + if (buttonDown) _yawStep(1); + } else if(action == _buttonActionGimbalCenter) { + if (buttonDown) { + _localPitch = 0.0; + _localYaw = 0.0; + emit gimbalControlValue(0.0, 0.0); + } + } else if(action == _buttonActionEmergencyStop) { + if(buttonDown) emit emergencyStop(); + } else if(action == _buttonActionGripperGrab) { + if(buttonDown) { + emit gripperAction(GRIPPER_ACTION_GRAB); + } + } else if(action == _buttonActionGripperRelease) { + if(buttonDown) { + emit gripperAction(GRIPPER_ACTION_RELEASE); + } + } else { + if (buttonDown && _activeVehicle) { + for (auto& item : _customMavCommands) { + if (action == item.name()) { + item.send(_activeVehicle); + return; + } + } + } + } +} + +void Joystick::_pitchStep(int direction) +{ + _localPitch += static_cast(direction); + //-- Arbitrary range + if(_localPitch < -90.0) _localPitch = -90.0; + if(_localPitch > 35.0) _localPitch = 35.0; + emit gimbalControlValue(_localPitch, _localYaw); +} + +void Joystick::_yawStep(int direction) +{ + _localYaw += static_cast(direction); + if(_localYaw < -180.0) _localYaw = -180.0; + if(_localYaw > 180.0) _localYaw = 180.0; + emit gimbalControlValue(_localPitch, _localYaw); +} + +bool Joystick::_validAxis(int axis) const +{ + if(axis >= 0 && axis < _axisCount) { + return true; + } + qCWarning(JoystickLog) << "Invalid axis index" << axis; + return false; +} + +bool Joystick::_validButton(int button) const +{ + if(button >= 0 && button < _totalButtonCount) + return true; + qCWarning(JoystickLog) << "Invalid button index" << button; + return false; +} + +int Joystick::_findAssignableButtonAction(const QString& action) +{ + for(int i = 0; i < _assignableButtonActions.count(); i++) { + AssignableButtonAction* p = qobject_cast(_assignableButtonActions[i]); + if(p->action() == action) + return i; + } + return -1; +} + +void Joystick::_buildActionList(Vehicle* activeVehicle) +{ + if(_assignableButtonActions.count()) + _assignableButtonActions.clearAndDeleteContents(); + _availableActionTitles.clear(); + //-- Available Actions + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNone)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionArm)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionDisarm)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionToggleArm)); + if (activeVehicle) { + QStringList list = activeVehicle->flightModes(); + foreach(auto mode, list) { + _assignableButtonActions.append(new AssignableButtonAction(this, mode)); + } + } + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionVTOLFixedWing)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionVTOLMultiRotor)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionContinuousZoomIn, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionContinuousZoomOut, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStepZoomIn, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStepZoomOut, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNextStream)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionPreviousStream)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNextCamera)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionPreviousCamera)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionTriggerCamera)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStartVideoRecord)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStopVideoRecord)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionToggleVideoRecord)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalDown, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalUp, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalLeft, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalRight, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalCenter)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease)); + + for (auto& item : _customMavCommands) { + _assignableButtonActions.append(new AssignableButtonAction(this, item.name())); + } + + for(int i = 0; i < _assignableButtonActions.count(); i++) { + AssignableButtonAction* p = qobject_cast(_assignableButtonActions[i]); + _availableActionTitles << p->action(); + } + emit assignableActionsChanged(); +} diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h new file mode 100644 index 0000000..7405630 --- /dev/null +++ b/src/Joystick/Joystick.h @@ -0,0 +1,370 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +/// @file +/// @brief Joystick Controller + +#pragma once + +#include +#include +#include + +#include "QGCLoggingCategory.h" +#include "Vehicle.h" +#include "MultiVehicleManager.h" +#include "JoystickMavCommand.h" + +// JoystickLog Category declaration moved to QGCLoggingCategory.cc to allow access in Vehicle +Q_DECLARE_LOGGING_CATEGORY(JoystickValuesLog) +Q_DECLARE_METATYPE(GRIPPER_ACTIONS) + +/// Action assigned to button +class AssignedButtonAction : public QObject { + Q_OBJECT +public: + AssignedButtonAction(QObject* parent, const QString name); + QString action; + QElapsedTimer buttonTime; + bool repeat = false; +}; + +/// Assignable Button Action +class AssignableButtonAction : public QObject { + Q_OBJECT +public: + AssignableButtonAction(QObject* parent, QString action_, bool canRepeat_ = false); + Q_PROPERTY(QString action READ action CONSTANT) + Q_PROPERTY(bool canRepeat READ canRepeat CONSTANT) + QString action () { return _action; } + bool canRepeat () const{ return _repeat; } +private: + QString _action; + bool _repeat = false; +}; + +/// Joystick Controller +class Joystick : public QThread +{ + Q_OBJECT +public: + Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager); + + virtual ~Joystick(); + + typedef struct Calibration_t { + int min; + int max; + int center; + int deadband; + bool reversed; + Calibration_t() + : min(-32767) + , max(32767) + , center(0) + , deadband(0) + , reversed(false) {} + } Calibration_t; + + typedef enum { + rollFunction, + pitchFunction, + yawFunction, + throttleFunction, + gimbalPitchFunction, + gimbalYawFunction, + maxFunction + } AxisFunction_t; + + typedef enum { + ThrottleModeCenterZero, + ThrottleModeDownZero, + ThrottleModeMax + } ThrottleMode_t; + + Q_PROPERTY(QString name READ name CONSTANT) + Q_PROPERTY(bool calibrated MEMBER _calibrated NOTIFY calibratedChanged) + Q_PROPERTY(int totalButtonCount READ totalButtonCount CONSTANT) + Q_PROPERTY(int axisCount READ axisCount CONSTANT) + Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT) + + //-- Actions assigned to buttons + Q_PROPERTY(QStringList buttonActions READ buttonActions NOTIFY buttonActionsChanged) + + //-- Actions that can be assigned to buttons + Q_PROPERTY(QmlObjectListModel* assignableActions READ assignableActions NOTIFY assignableActionsChanged) + Q_PROPERTY(QStringList assignableActionTitles READ assignableActionTitles NOTIFY assignableActionsChanged) + Q_PROPERTY(QString disabledActionName READ disabledActionName CONSTANT) + + Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged) + Q_PROPERTY(float axisFrequencyHz READ axisFrequencyHz WRITE setAxisFrequency NOTIFY axisFrequencyHzChanged) + Q_PROPERTY(float minAxisFrequencyHz MEMBER _minAxisFrequencyHz CONSTANT) + Q_PROPERTY(float maxAxisFrequencyHz MEMBER _minAxisFrequencyHz CONSTANT) + Q_PROPERTY(float buttonFrequencyHz READ buttonFrequencyHz WRITE setButtonFrequency NOTIFY buttonFrequencyHzChanged) + Q_PROPERTY(float minButtonFrequencyHz MEMBER _minButtonFrequencyHz CONSTANT) + Q_PROPERTY(float maxButtonFrequencyHz MEMBER _maxButtonFrequencyHz CONSTANT) + Q_PROPERTY(bool negativeThrust READ negativeThrust WRITE setNegativeThrust NOTIFY negativeThrustChanged) + Q_PROPERTY(float exponential READ exponential WRITE setExponential NOTIFY exponentialChanged) + Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged) + Q_PROPERTY(bool circleCorrection READ circleCorrection WRITE setCircleCorrection NOTIFY circleCorrectionChanged) + + Q_INVOKABLE void setButtonRepeat (int button, bool repeat); + Q_INVOKABLE bool getButtonRepeat (int button); + Q_INVOKABLE void setButtonAction (int button, const QString& action); + Q_INVOKABLE QString getButtonAction (int button); + + // Property accessors + + QString name () { return _name; } + int totalButtonCount () const{ return _totalButtonCount; } + int axisCount () const{ return _axisCount; } + QStringList buttonActions (); + + QmlObjectListModel* assignableActions () { return &_assignableButtonActions; } + QStringList assignableActionTitles () { return _availableActionTitles; } + QString disabledActionName () { return _buttonActionNone; } + + /// Start the polling thread which will in turn emit joystick signals + void startPolling(Vehicle* vehicle); + void stopPolling(void); + + void setCalibration(int axis, Calibration_t& calibration); + Calibration_t getCalibration(int axis); + + void setFunctionAxis(AxisFunction_t function, int axis); + int getFunctionAxis(AxisFunction_t function); + + void stop(); + +/* + // Joystick index used by sdl library + // Settable because sdl library remaps indices after certain events + virtual int index(void) = 0; + virtual void setIndex(int index) = 0; +*/ + virtual bool requiresCalibration(void) { return true; } + + int throttleMode (); + void setThrottleMode (int mode); + + bool negativeThrust () const; + void setNegativeThrust (bool allowNegative); + + float exponential () const; + void setExponential (float expo); + + bool accumulator () const; + void setAccumulator (bool accu); + + bool deadband () const; + void setDeadband (bool accu); + + bool circleCorrection () const; + void setCircleCorrection(bool circleCorrection); + + void setTXMode (int mode); + int getTXMode () { return _transmitterMode; } + + /// Set the current calibration mode + void setCalibrationMode (bool calibrating); + + /// Get joystick message rate (in Hz) + float axisFrequencyHz () const{ return _axisFrequencyHz; } + /// Set joystick message rate (in Hz) + void setAxisFrequency (float val); + + /// Get joystick button repeat rate (in Hz) + float buttonFrequencyHz () const{ return _buttonFrequencyHz; } + /// Set joystick button repeat rate (in Hz) + void setButtonFrequency(float val); + +signals: + // The raw signals are only meant for use by calibration + void rawAxisValueChanged (int index, int value); + void rawButtonPressedChanged (int index, int pressed); + void calibratedChanged (bool calibrated); + void buttonActionsChanged (); + void assignableActionsChanged (); + void throttleModeChanged (int mode); + void negativeThrustChanged (bool allowNegative); + void exponentialChanged (float exponential); + void accumulatorChanged (bool accumulator); + void enabledChanged (bool enabled); + void circleCorrectionChanged (bool circleCorrection); + void axisValues (float roll, float pitch, float yaw, float throttle); + + void axisFrequencyHzChanged (); + void buttonFrequencyHzChanged (); + void startContinuousZoom (int direction); + void stopContinuousZoom (); + void stepZoom (int direction); + void stepCamera (int direction); + void stepStream (int direction); + void triggerCamera (); + void startVideoRecord (); + void stopVideoRecord (); + void toggleVideoRecord (); + void gimbalPitchStep (int direction); + void gimbalYawStep (int direction); + void centerGimbal (); + void gimbalControlValue (double pitch, double yaw); + void setArmed (bool arm); + void setVtolInFwdFlight (bool set); + void setFlightMode (const QString& flightMode); + void emergencyStop (); + /** + * @brief Send MAV_CMD_DO_GRIPPER command to the vehicle + * + * @param gripperAction (Open / Close) Gripper action to command + */ + void gripperAction (GRIPPER_ACTIONS gripperAction); + +protected: + void _setDefaultCalibration (); + void _saveSettings (); + void _saveButtonSettings (); + void _loadSettings (); + float _adjustRange (int value, Calibration_t calibration, bool withDeadbands); + void _executeButtonAction (const QString& action, bool buttonDown); + int _findAssignableButtonAction(const QString& action); + bool _validAxis (int axis) const; + bool _validButton (int button) const; + void _handleAxis (); + void _handleButtons (); + void _buildActionList (Vehicle* activeVehicle); + + void _pitchStep (int direction); + void _yawStep (int direction); + double _localYaw = 0.0; + double _localPitch = 0.0; + +private: + virtual bool _open () = 0; + virtual void _close () = 0; + virtual bool _update () = 0; + + virtual bool _getButton (int i) = 0; + virtual int _getAxis (int i) = 0; + virtual bool _getHat (int hat,int i) = 0; + + void _updateTXModeSettingsKey(Vehicle* activeVehicle); + int _mapFunctionMode(int mode, int function); + void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]); + + // Override from QThread + virtual void run(); + +protected: + + enum { + BUTTON_UP, + BUTTON_DOWN, + BUTTON_REPEAT + }; + + static const float _defaultAxisFrequencyHz; + static const float _defaultButtonFrequencyHz; + + uint8_t*_rgButtonValues = nullptr; + + std::atomic _exitThread{false}; ///< true: signal thread to exit + bool _calibrationMode = false; + int* _rgAxisValues = nullptr; + Calibration_t* _rgCalibration = nullptr; + ThrottleMode_t _throttleMode = ThrottleModeDownZero; + bool _negativeThrust = false; + float _exponential = 0; + bool _accumulator = false; + bool _deadband = false; + bool _circleCorrection = true; + float _axisFrequencyHz = _defaultAxisFrequencyHz; + float _buttonFrequencyHz = _defaultButtonFrequencyHz; + Vehicle* _activeVehicle = nullptr; + + bool _pollingStartedForCalibration = false; + + QString _name; + bool _calibrated; + int _axisCount; + int _buttonCount; + int _hatCount; + int _hatButtonCount; + int _totalButtonCount; + + static int _transmitterMode; + int _rgFunctionAxis[maxFunction] = {}; + QElapsedTimer _axisTime; + + QmlObjectListModel _assignableButtonActions; + QList _buttonActionArray; + QStringList _availableActionTitles; + MultiVehicleManager* _multiVehicleManager = nullptr; + + QList _customMavCommands; + + static const float _minAxisFrequencyHz; + static const float _maxAxisFrequencyHz; + static const float _minButtonFrequencyHz; + static const float _maxButtonFrequencyHz; + +private: + static const char* _rgFunctionSettingsKey[maxFunction]; + + static const char* _settingsGroup; + static const char* _calibratedSettingsKey; + static const char* _buttonActionNameKey; + static const char* _buttonActionRepeatKey; + static const char* _throttleModeSettingsKey; + static const char* _negativeThrustSettingsKey; + static const char* _exponentialSettingsKey; + static const char* _accumulatorSettingsKey; + static const char* _deadbandSettingsKey; + static const char* _circleCorrectionSettingsKey; + static const char* _axisFrequencySettingsKey; + static const char* _buttonFrequencySettingsKey; + static const char* _txModeSettingsKey; + static const char* _fixedWingTXModeSettingsKey; + static const char* _multiRotorTXModeSettingsKey; + static const char* _roverTXModeSettingsKey; + static const char* _vtolTXModeSettingsKey; + static const char* _submarineTXModeSettingsKey; + + static const char* _buttonActionNone; + static const char* _buttonActionArm; + static const char* _buttonActionDisarm; + static const char* _buttonActionToggleArm; + static const char* _buttonActionVTOLFixedWing; + static const char* _buttonActionVTOLMultiRotor; + static const char* _buttonActionStepZoomIn; + static const char* _buttonActionStepZoomOut; + static const char* _buttonActionContinuousZoomIn; + static const char* _buttonActionContinuousZoomOut; + static const char* _buttonActionNextStream; + static const char* _buttonActionPreviousStream; + static const char* _buttonActionNextCamera; + static const char* _buttonActionPreviousCamera; + static const char* _buttonActionTriggerCamera; + static const char* _buttonActionStartVideoRecord; + static const char* _buttonActionStopVideoRecord; + static const char* _buttonActionToggleVideoRecord; + static const char* _buttonActionGimbalDown; + static const char* _buttonActionGimbalUp; + static const char* _buttonActionGimbalLeft; + static const char* _buttonActionGimbalRight; + static const char* _buttonActionGimbalCenter; + static const char* _buttonActionEmergencyStop; + static const char* _buttonActionGripperGrab; + static const char* _buttonActionGripperRelease; + + +private slots: + void _activeVehicleChanged(Vehicle* activeVehicle); + void _vehicleCountChanged(int count); + void _flightModesChanged(); +}; diff --git a/src/Joystick/JoystickAndroid.cc b/src/Joystick/JoystickAndroid.cc new file mode 100644 index 0000000..3994864 --- /dev/null +++ b/src/Joystick/JoystickAndroid.cc @@ -0,0 +1,293 @@ +#include "JoystickAndroid.h" + +#include "QGCApplication.h" + +#include + +int JoystickAndroid::_androidBtnListCount; +int *JoystickAndroid::_androidBtnList; +int JoystickAndroid::ACTION_DOWN; +int JoystickAndroid::ACTION_UP; +QMutex JoystickAndroid::m_mutex; + +static void clear_jni_exception() +{ + QAndroidJniEnvironment jniEnv; + if (jniEnv->ExceptionCheck()) { + jniEnv->ExceptionDescribe(); + jniEnv->ExceptionClear(); + } +} + +JoystickAndroid::JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id, MultiVehicleManager* multiVehicleManager) + : Joystick(name,axisCount,buttonCount,0,multiVehicleManager) + , deviceId(id) +{ + int i; + + QAndroidJniEnvironment env; + QAndroidJniObject inputDevice = QAndroidJniObject::callStaticObjectMethod("android/view/InputDevice", "getDevice", "(I)Landroid/view/InputDevice;", id); + + //set button mapping (number->code) + jintArray b = env->NewIntArray(_androidBtnListCount); + env->SetIntArrayRegion(b,0,_androidBtnListCount,_androidBtnList); + + QAndroidJniObject btns = inputDevice.callObjectMethod("hasKeys", "([I)[Z", b); + jbooleanArray jSupportedButtons = btns.object(); + jboolean* supportedButtons = env->GetBooleanArrayElements(jSupportedButtons, nullptr); + //create a mapping table (btnCode) that maps button number with button code + btnValue = new bool[_buttonCount]; + btnCode = new int[_buttonCount]; + int c = 0; + for (i = 0; i < _androidBtnListCount; i++) { + if (supportedButtons[i]) { + btnValue[c] = false; + btnCode[c] = _androidBtnList[i]; + c++; + } + } + + env->ReleaseBooleanArrayElements(jSupportedButtons, supportedButtons, 0); + + // set axis mapping (number->code) + axisValue = new int[_axisCount]; + axisCode = new int[_axisCount]; + QAndroidJniObject rangeListNative = inputDevice.callObjectMethod("getMotionRanges", "()Ljava/util/List;"); + for (i = 0; i < _axisCount; i++) { + QAndroidJniObject range = rangeListNative.callObjectMethod("get", "(I)Ljava/lang/Object;",i); + axisCode[i] = range.callMethod("getAxis"); + // Don't allow two axis with the same code + for (int j = 0; j < i; j++) { + if (axisCode[i] == axisCode[j]) { + axisCode[i] = -1; + break; + } + } + axisValue[i] = 0; + } + + qCDebug(JoystickLog) << "axis:" <<_axisCount << "buttons:" <<_buttonCount; + QtAndroidPrivate::registerGenericMotionEventListener(this); + QtAndroidPrivate::registerKeyEventListener(this); +} + +JoystickAndroid::~JoystickAndroid() { + delete btnCode; + delete axisCode; + delete btnValue; + delete axisValue; + + QtAndroidPrivate::unregisterGenericMotionEventListener(this); + QtAndroidPrivate::unregisterKeyEventListener(this); +} + + +QMap JoystickAndroid::discover(MultiVehicleManager* _multiVehicleManager) { + static QMap ret; + + QMutexLocker lock(&m_mutex); + + QAndroidJniEnvironment env; + QAndroidJniObject o = QAndroidJniObject::callStaticObjectMethod("android/view/InputDevice", "getDeviceIds"); + jintArray jarr = o.object(); + int sz = env->GetArrayLength(jarr); + jint *buff = env->GetIntArrayElements(jarr, nullptr); + + int SOURCE_GAMEPAD = QAndroidJniObject::getStaticField("android/view/InputDevice", "SOURCE_GAMEPAD"); + int SOURCE_JOYSTICK = QAndroidJniObject::getStaticField("android/view/InputDevice", "SOURCE_JOYSTICK"); + + QList names; + + for (int i = 0; i < sz; ++i) { + QAndroidJniObject inputDevice = QAndroidJniObject::callStaticObjectMethod("android/view/InputDevice", "getDevice", "(I)Landroid/view/InputDevice;", buff[i]); + int sources = inputDevice.callMethod("getSources", "()I"); + if (((sources & SOURCE_GAMEPAD) != SOURCE_GAMEPAD) //check if the input device is interesting to us + && ((sources & SOURCE_JOYSTICK) != SOURCE_JOYSTICK)) continue; + + // get id and name + QString id = inputDevice.callObjectMethod("getDescriptor", "()Ljava/lang/String;").toString(); + QString name = inputDevice.callObjectMethod("getName", "()Ljava/lang/String;").toString(); + + names.push_back(name); + + if (ret.contains(name)) { + continue; + } + + // get number of axis + QAndroidJniObject rangeListNative = inputDevice.callObjectMethod("getMotionRanges", "()Ljava/util/List;"); + int axisCount = rangeListNative.callMethod("size"); + + // get number of buttons + jintArray a = env->NewIntArray(_androidBtnListCount); + env->SetIntArrayRegion(a,0,_androidBtnListCount,_androidBtnList); + QAndroidJniObject btns = inputDevice.callObjectMethod("hasKeys", "([I)[Z", a); + jbooleanArray jSupportedButtons = btns.object(); + jboolean* supportedButtons = env->GetBooleanArrayElements(jSupportedButtons, nullptr); + int buttonCount = 0; + for (int j=0;j<_androidBtnListCount;j++) + if (supportedButtons[j]) buttonCount++; + env->ReleaseBooleanArrayElements(jSupportedButtons, supportedButtons, 0); + + qCDebug(JoystickLog) << "\t" << name << "id:" << buff[i] << "axes:" << axisCount << "buttons:" << buttonCount; + + ret[name] = new JoystickAndroid(name, axisCount, buttonCount, buff[i], _multiVehicleManager); + } + + for (auto i = ret.begin(); i != ret.end();) { + if (!names.contains(i.key())) { + i = ret.erase(i); + } else { + i++; + } + } + + env->ReleaseIntArrayElements(jarr, buff, 0); + + return ret; +} + + +bool JoystickAndroid::handleKeyEvent(jobject event) { + QJNIObjectPrivate ev(event); + QMutexLocker lock(&m_mutex); + const int _deviceId = ev.callMethod("getDeviceId", "()I"); + if (_deviceId!=deviceId) return false; + + const int action = ev.callMethod("getAction", "()I"); + const int keyCode = ev.callMethod("getKeyCode", "()I"); + + for (int i = 0; i <_buttonCount; i++) { + if (btnCode[i] == keyCode) { + if (action == ACTION_DOWN) btnValue[i] = true; + if (action == ACTION_UP) btnValue[i] = false; + return true; + } + } + return false; +} + +bool JoystickAndroid::handleGenericMotionEvent(jobject event) { + QJNIObjectPrivate ev(event); + QMutexLocker lock(&m_mutex); + const int _deviceId = ev.callMethod("getDeviceId", "()I"); + if (_deviceId!=deviceId) return false; + + for (int i = 0; i <_axisCount; i++) { + const float v = ev.callMethod("getAxisValue", "(I)F",axisCode[i]); + axisValue[i] = static_cast((v*32767.f)); + } + return true; +} + +bool JoystickAndroid::_open(void) { + return true; +} + +void JoystickAndroid::_close(void) { +} + +bool JoystickAndroid::_update(void) +{ + return true; +} + +bool JoystickAndroid::_getButton(int i) { + return btnValue[ i ]; +} + +int JoystickAndroid::_getAxis(int i) { + return axisValue[ i ]; +} + +bool JoystickAndroid::_getHat(int hat,int i) { + Q_UNUSED(hat); + Q_UNUSED(i); + return false; +} + +static JoystickManager *_manager = nullptr; + +//helper method +bool JoystickAndroid::init(JoystickManager *manager) { + _manager = manager; + + //this gets list of all possible buttons - this is needed to check how many buttons our gamepad supports + //instead of the whole logic below we could have just a simple array of hardcoded int values as these 'should' not change + + //int JoystickAndroid::_androidBtnListCount; + _androidBtnListCount = 31; + static int ret[31]; //there are 31 buttons in total accordingy to the API + int i; + //int *JoystickAndroid:: + _androidBtnList = ret; + + clear_jni_exception(); + for (i = 1; i <= 16; i++) { + QString name = "KEYCODE_BUTTON_"+QString::number(i); + ret[i-1] = QAndroidJniObject::getStaticField("android/view/KeyEvent", name.toStdString().c_str()); + } + i--; + + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_A"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_B"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_C"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_L1"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_L2"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_R1"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_R2"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_MODE"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_SELECT"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_START"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_THUMBL"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_THUMBR"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_X"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_Y"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_Z"); + + ACTION_DOWN = QAndroidJniObject::getStaticField("android/view/KeyEvent", "ACTION_DOWN"); + ACTION_UP = QAndroidJniObject::getStaticField("android/view/KeyEvent", "ACTION_UP"); + + return true; +} + +static const char kJniClassName[] {"org/mavlink/qgroundcontrol/QGCActivity"}; + +static void jniUpdateAvailableJoysticks(JNIEnv *envA, jobject thizA) +{ + Q_UNUSED(envA); + Q_UNUSED(thizA); + + if (_manager != nullptr) { + qCDebug(JoystickLog) << "jniUpdateAvailableJoysticks triggered"; + emit _manager->updateAvailableJoysticksSignal(); + } +} + +void JoystickAndroid::setNativeMethods() +{ + qCDebug(JoystickLog) << "Registering Native Functions"; + + // REGISTER THE C++ FUNCTION WITH JNI + JNINativeMethod javaMethods[] { + {"nativeUpdateAvailableJoysticks", "()V", reinterpret_cast(jniUpdateAvailableJoysticks)} + }; + + clear_jni_exception(); + QAndroidJniEnvironment jniEnv; + jclass objectClass = jniEnv->FindClass(kJniClassName); + if(!objectClass) { + clear_jni_exception(); + qWarning() << "Couldn't find class:" << kJniClassName; + return; + } + + jint val = jniEnv->RegisterNatives(objectClass, javaMethods, sizeof(javaMethods) / sizeof(javaMethods[0])); + + if (val < 0) { + qWarning() << "Error registering methods: " << val; + } else { + qCDebug(JoystickLog) << "Native Functions Registered"; + } + clear_jni_exception(); +} diff --git a/src/Joystick/JoystickAndroid.h b/src/Joystick/JoystickAndroid.h new file mode 100644 index 0000000..2ff473c --- /dev/null +++ b/src/Joystick/JoystickAndroid.h @@ -0,0 +1,54 @@ +#ifndef JOYSTICKANDROID_H +#define JOYSTICKANDROID_H + +#include "Joystick.h" +#include "Vehicle.h" +#include "MultiVehicleManager.h" + +#include +#include +#include +#include +#include + + +class JoystickAndroid : public Joystick, public QtAndroidPrivate::GenericMotionEventListener, public QtAndroidPrivate::KeyEventListener +{ +public: + JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id, MultiVehicleManager* multiVehicleManager); + + ~JoystickAndroid(); + + static bool init(JoystickManager *manager); + + static void setNativeMethods(); + + static QMap discover(MultiVehicleManager* _multiVehicleManager); + +private: + bool handleKeyEvent(jobject event); + bool handleGenericMotionEvent(jobject event); + + virtual bool _open (); + virtual void _close (); + virtual bool _update (); + + virtual bool _getButton (int i); + virtual int _getAxis (int i); + virtual bool _getHat (int hat,int i); + + int *btnCode; + int *axisCode; + bool *btnValue; + int *axisValue; + + static int * _androidBtnList; //list of all possible android buttons + static int _androidBtnListCount; + + static int ACTION_DOWN, ACTION_UP; + static QMutex m_mutex; + + int deviceId; +}; + +#endif // JOYSTICKANDROID_H diff --git a/src/Joystick/JoystickManager.cc b/src/Joystick/JoystickManager.cc new file mode 100644 index 0000000..bd03f15 --- /dev/null +++ b/src/Joystick/JoystickManager.cc @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +#include "JoystickManager.h" +#include "QGCApplication.h" + +#include + +#ifndef __mobile__ + #include "JoystickSDL.h" + #define __sdljoystick__ +#endif + +#ifdef __android__ + #include "JoystickAndroid.h" +#endif + +QGC_LOGGING_CATEGORY(JoystickManagerLog, "JoystickManagerLog") + +const char * JoystickManager::_settingsGroup = "JoystickManager"; +const char * JoystickManager::_settingsKeyActiveJoystick = "ActiveJoystick"; + +JoystickManager::JoystickManager(QGCApplication* app, QGCToolbox* toolbox) + : QGCTool(app, toolbox) + , _activeJoystick(nullptr) + , _multiVehicleManager(nullptr) +{ +} + +JoystickManager::~JoystickManager() { + QMap::iterator i; + for (i = _name2JoystickMap.begin(); i != _name2JoystickMap.end(); ++i) { + qCDebug(JoystickManagerLog) << "Releasing joystick:" << i.key(); + i.value()->stop(); + delete i.value(); + } + qDebug() << "Done"; +} + +void JoystickManager::setToolbox(QGCToolbox *toolbox) +{ + QGCTool::setToolbox(toolbox); + + _multiVehicleManager = _toolbox->multiVehicleManager(); + + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); +} + +void JoystickManager::init() { +#ifdef __sdljoystick__ + if (!JoystickSDL::init()) { + return; + } + _setActiveJoystickFromSettings(); +#elif defined(__android__) + if (!JoystickAndroid::init(this)) { + return; + } + connect(this, &JoystickManager::updateAvailableJoysticksSignal, this, &JoystickManager::restartJoystickCheckTimer); +#endif + connect(&_joystickCheckTimer, &QTimer::timeout, this, &JoystickManager::_updateAvailableJoysticks); + _joystickCheckTimerCounter = 5; + _joystickCheckTimer.start(1000); +} + +void JoystickManager::_setActiveJoystickFromSettings(void) +{ + QMap newMap; + +#ifdef __sdljoystick__ + // Get the latest joystick mapping + newMap = JoystickSDL::discover(_multiVehicleManager); +#elif defined(__android__) + newMap = JoystickAndroid::discover(_multiVehicleManager); +#endif + + if (_activeJoystick && !newMap.contains(_activeJoystick->name())) { + qCDebug(JoystickManagerLog) << "Active joystick removed"; + setActiveJoystick(nullptr); + } + + // Check to see if our current mapping contains any joysticks that are not in the new mapping + // If so, those joysticks have been unplugged, and need to be cleaned up + QMap::iterator i; + for (i = _name2JoystickMap.begin(); i != _name2JoystickMap.end(); ++i) { + if (!newMap.contains(i.key())) { + qCDebug(JoystickManagerLog) << "Releasing joystick:" << i.key(); + i.value()->stopPolling(); + i.value()->wait(1000); + i.value()->deleteLater(); + } + } + + _name2JoystickMap = newMap; + emit availableJoysticksChanged(); + + if (!_name2JoystickMap.count()) { + setActiveJoystick(nullptr); + return; + } + + QSettings settings; + + settings.beginGroup(_settingsGroup); + QString name = settings.value(_settingsKeyActiveJoystick).toString(); + + if (name.isEmpty()) { + name = _name2JoystickMap.first()->name(); + } + + setActiveJoystick(_name2JoystickMap.value(name, _name2JoystickMap.first())); + settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name()); +} + +Joystick* JoystickManager::activeJoystick(void) +{ + return _activeJoystick; +} + +void JoystickManager::setActiveJoystick(Joystick* joystick) +{ + QSettings settings; + + if (joystick != nullptr && !_name2JoystickMap.contains(joystick->name())) { + qCWarning(JoystickManagerLog) << "Set active not in map" << joystick->name(); + return; + } + + if (_activeJoystick == joystick) { + return; + } + + if (_activeJoystick) { + _activeJoystick->stopPolling(); + } + + _activeJoystick = joystick; + + if (_activeJoystick != nullptr) { + qCDebug(JoystickManagerLog) << "Set active:" << _activeJoystick->name(); + + settings.beginGroup(_settingsGroup); + settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name()); + } + + emit activeJoystickChanged(_activeJoystick); + emit activeJoystickNameChanged(_activeJoystick?_activeJoystick->name():""); +} + +QVariantList JoystickManager::joysticks(void) +{ + QVariantList list; + + for (const QString &name: _name2JoystickMap.keys()) { + list += QVariant::fromValue(_name2JoystickMap[name]); + } + + return list; +} + +QStringList JoystickManager::joystickNames(void) +{ + return _name2JoystickMap.keys(); +} + +QString JoystickManager::activeJoystickName(void) +{ + return _activeJoystick ? _activeJoystick->name() : QString(); +} + +bool JoystickManager::setActiveJoystickName(const QString& name) +{ + if (_name2JoystickMap.contains(name)) { + setActiveJoystick(_name2JoystickMap[name]); + return true; + } else { + qCWarning(JoystickManagerLog) << "Set active not in map" << name; + return false; + } +} + +/* + * TODO: move this to the right place: JoystickSDL.cc and JoystickAndroid.cc respectively and call through Joystick.cc + */ +void JoystickManager::_updateAvailableJoysticks() +{ +#ifdef __sdljoystick__ + SDL_Event event; + while (SDL_PollEvent(&event)) { + switch(event.type) { + case SDL_QUIT: + qCDebug(JoystickManagerLog) << "SDL ERROR:" << SDL_GetError(); + break; + case SDL_JOYDEVICEADDED: + qCDebug(JoystickManagerLog) << "Joystick added:" << event.jdevice.which; + _setActiveJoystickFromSettings(); + break; + case SDL_JOYDEVICEREMOVED: + qCDebug(JoystickManagerLog) << "Joystick removed:" << event.jdevice.which; + _setActiveJoystickFromSettings(); + break; + default: + break; + } + } +#elif defined(__android__) + _joystickCheckTimerCounter--; + _setActiveJoystickFromSettings(); + if (_joystickCheckTimerCounter <= 0) { + _joystickCheckTimer.stop(); + } +#endif +} + +void JoystickManager::restartJoystickCheckTimer() +{ + _joystickCheckTimerCounter = 5; + _joystickCheckTimer.start(1000); +} diff --git a/src/Joystick/JoystickManager.h b/src/Joystick/JoystickManager.h new file mode 100644 index 0000000..6614501 --- /dev/null +++ b/src/Joystick/JoystickManager.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +/// @file +/// @brief Joystick Manager + +#pragma once + +#include "QGCLoggingCategory.h" +#include "Joystick.h" +#include "MultiVehicleManager.h" +#include "QGCToolbox.h" + +#include + +Q_DECLARE_LOGGING_CATEGORY(JoystickManagerLog) + +/// Joystick Manager +class JoystickManager : public QGCTool +{ + Q_OBJECT + +public: + JoystickManager(QGCApplication* app, QGCToolbox* toolbox); + ~JoystickManager(); + + Q_PROPERTY(QVariantList joysticks READ joysticks NOTIFY availableJoysticksChanged) + Q_PROPERTY(QStringList joystickNames READ joystickNames NOTIFY availableJoysticksChanged) + + Q_PROPERTY(Joystick* activeJoystick READ activeJoystick WRITE setActiveJoystick NOTIFY activeJoystickChanged) + Q_PROPERTY(QString activeJoystickName READ activeJoystickName WRITE setActiveJoystickName NOTIFY activeJoystickNameChanged) + + /// List of available joysticks + QVariantList joysticks(); + /// List of available joystick names + QStringList joystickNames(void); + + /// Get active joystick + Joystick* activeJoystick(void); + /// Set active joystick + void setActiveJoystick(Joystick* joystick); + + QString activeJoystickName(void); + bool setActiveJoystickName(const QString& name); + + void restartJoystickCheckTimer(void); + + // Override from QGCTool + virtual void setToolbox(QGCToolbox *toolbox); + +public slots: + void init(); + +signals: + void activeJoystickChanged(Joystick* joystick); + void activeJoystickNameChanged(const QString& name); + void availableJoysticksChanged(void); + void updateAvailableJoysticksSignal(); + +private slots: + void _updateAvailableJoysticks(void); + +private: + void _setActiveJoystickFromSettings(void); + +private: + Joystick* _activeJoystick; + QMap _name2JoystickMap; + MultiVehicleManager* _multiVehicleManager; + + static const char * _settingsGroup; + static const char * _settingsKeyActiveJoystick; + + int _joystickCheckTimerCounter; + QTimer _joystickCheckTimer; +}; diff --git a/src/Joystick/JoystickMavCommand.cc b/src/Joystick/JoystickMavCommand.cc new file mode 100644 index 0000000..66e72f4 --- /dev/null +++ b/src/Joystick/JoystickMavCommand.cc @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "JoystickMavCommand.h" +#include "QGCLoggingCategory.h" +#include "Vehicle.h" +#include +#include +#include + +QGC_LOGGING_CATEGORY(JoystickMavCommandLog, "JoystickMavCommandLog") + +static void parseJsonValue(const QJsonObject& jsonObject, const QString& key, float& param) +{ + if (jsonObject.contains(key)) + param = static_cast(jsonObject.value(key).toDouble()); +} + +QList JoystickMavCommand::load(const QString& jsonFilename) +{ + qCDebug(JoystickMavCommandLog) << "Loading" << jsonFilename; + QList result; + + QFile jsonFile(jsonFilename); + if (!jsonFile.open(QIODevice::ReadOnly | QIODevice::Text)) { + qCDebug(JoystickMavCommandLog) << "Could not open" << jsonFilename; + return result; + } + + QByteArray bytes = jsonFile.readAll(); + jsonFile.close(); + QJsonParseError jsonParseError; + QJsonDocument doc = QJsonDocument::fromJson(bytes, &jsonParseError); + if (jsonParseError.error != QJsonParseError::NoError) { + qWarning() << jsonFilename << "Unable to open json document" << jsonParseError.errorString(); + return result; + } + + QJsonObject json = doc.object(); + + const int version = json.value("version").toInt(); + if (version != 1) { + qWarning() << jsonFilename << ": invalid version" << version; + return result; + } + + QJsonValue jsonValue = json.value("commands"); + if (!jsonValue.isArray()) { + qWarning() << jsonFilename << ": 'commands' is not an array"; + return result; + } + + QJsonArray jsonArray = jsonValue.toArray(); + for (QJsonValue info: jsonArray) { + if (!info.isObject()) { + qWarning() << jsonFilename << ": 'commands' should contain objects"; + return result; + } + + auto jsonObject = info.toObject(); + JoystickMavCommand item; + if (!jsonObject.contains("id")) { + qWarning() << jsonFilename << ": 'id' is required"; + continue; + } + item._id = jsonObject.value("id").toInt(); + if (!jsonObject.contains("name")) { + qWarning() << jsonFilename << ": 'name' is required"; + continue; + } + item._name = jsonObject.value("name").toString(); + item._showError = jsonObject.value("showError").toBool(); + parseJsonValue(jsonObject, "param1", item._param1); + parseJsonValue(jsonObject, "param2", item._param2); + parseJsonValue(jsonObject, "param3", item._param3); + parseJsonValue(jsonObject, "param4", item._param4); + parseJsonValue(jsonObject, "param5", item._param5); + parseJsonValue(jsonObject, "param6", item._param6); + parseJsonValue(jsonObject, "param7", item._param7); + + qCDebug(JoystickMavCommandLog) << jsonObject; + + result.append(item); + } + + return result; +} + +void JoystickMavCommand::send(Vehicle* vehicle) +{ + vehicle->sendMavCommand(vehicle->defaultComponentId(), + static_cast(_id), + _showError, + _param1, _param2, _param3, _param4, _param5, _param6, _param7); +} diff --git a/src/Joystick/JoystickMavCommand.h b/src/Joystick/JoystickMavCommand.h new file mode 100644 index 0000000..96cf9f4 --- /dev/null +++ b/src/Joystick/JoystickMavCommand.h @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +/// @file +/// @brief Custom Joystick MAV command + +#pragma once + +#include +#include + +class Vehicle; + +/// Custom MAV command +class JoystickMavCommand +{ +public: + static QList load(const QString& jsonFilename); + QString name() const { return _name; } + + void send(Vehicle* vehicle); +private: + QString _name; + int _id = 0; + bool _showError = false; + float _param1 = 0.0f; + float _param2 = 0.0f; + float _param3 = 0.0f; + float _param4 = 0.0f; + float _param5 = 0.0f; + float _param6 = 0.0f; + float _param7 = 0.0f; +}; + diff --git a/src/Joystick/JoystickMavCommands.json b/src/Joystick/JoystickMavCommands.json new file mode 100644 index 0000000..ba28ed4 --- /dev/null +++ b/src/Joystick/JoystickMavCommands.json @@ -0,0 +1,17 @@ +{ + "comment": "Joystick MAV commands", + "version": 1, + + "commands": [ + { + "id": 31010, + "name": "MAV_CMD_USER_1", + "param1": 1.0 + }, + { + "id": 31011, + "name": "MAV_CMD_USER_2", + "param1": 0.0 + } + ] +} diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc new file mode 100644 index 0000000..4743e38 --- /dev/null +++ b/src/Joystick/JoystickSDL.cc @@ -0,0 +1,178 @@ +#include "JoystickSDL.h" + +#include "QGCApplication.h" + +#include +#include + +JoystickSDL::JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController, MultiVehicleManager* multiVehicleManager) + : Joystick(name,axisCount,buttonCount,hatCount,multiVehicleManager) + , _isGameController(isGameController) + , _index(index) +{ + if(_isGameController) _setDefaultCalibration(); +} + +bool JoystickSDL::init(void) { + if (SDL_InitSubSystem(SDL_INIT_GAMECONTROLLER | SDL_INIT_JOYSTICK) < 0) { + SDL_JoystickEventState(SDL_ENABLE); + qWarning() << "Couldn't initialize SimpleDirectMediaLayer:" << SDL_GetError(); + return false; + } + _loadGameControllerMappings(); + return true; +} + +QMap JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) { + static QMap ret; + + QMap newRet; + + // Load available joysticks + + qCDebug(JoystickLog) << "Available joysticks"; + + for (int i=0; i(newRet[name]); + if (j->index() != i) { + j->setIndex(i); // This joystick index has been remapped by SDL + } + // Anything left in ret after we exit the loop has been removed (unplugged) and needs to be cleaned up. + // We will handle that in JoystickManager in case the removed joystick was in use. + ret.remove(name); + qCDebug(JoystickLog) << "\tSkipping duplicate" << name; + } + } + + if (!newRet.count()) { + qCDebug(JoystickLog) << "\tnone found"; + } + + ret = newRet; + return ret; +} + +void JoystickSDL::_loadGameControllerMappings(void) { + QFile file(":/db/mapping/joystick/gamecontrollerdb.txt"); + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) + { + qWarning() << "Couldn't load GameController mapping database."; + return; + } + + QTextStream s(&file); + + while (!s.atEnd()) { + SDL_GameControllerAddMapping(s.readLine().toStdString().c_str()); + } +} + +bool JoystickSDL::_open(void) { + if ( _isGameController ) { + sdlController = SDL_GameControllerOpen(_index); + sdlJoystick = SDL_GameControllerGetJoystick(sdlController); + } else { + sdlJoystick = SDL_JoystickOpen(_index); + } + + if (!sdlJoystick) { + qCWarning(JoystickLog) << "SDL_JoystickOpen failed:" << SDL_GetError(); + return false; + } + + qCDebug(JoystickLog) << "Opened joystick at" << sdlJoystick; + + return true; +} + +void JoystickSDL::_close(void) { + if (sdlJoystick == nullptr) { + qCDebug(JoystickLog) << "Attempt to close null joystick!"; + return; + } + + qCDebug(JoystickLog) << "Closing" << SDL_JoystickName(sdlJoystick) << "at" << sdlJoystick; + + // We get a segfault if we try to close a joystick that has been detached + if (SDL_JoystickGetAttached(sdlJoystick) == SDL_FALSE) { + qCDebug(JoystickLog) << "\tJoystick is not attached!"; + } else { + + if (SDL_JoystickInstanceID(sdlJoystick) != -1) { + qCDebug(JoystickLog) << "\tID:" << SDL_JoystickInstanceID(sdlJoystick); + // This segfaults so often, and I've spent so much time trying to find the cause and fix it + // I think this might be an SDL bug + // We are much more stable just commenting this out + //SDL_JoystickClose(sdlJoystick); + } + } + + sdlJoystick = nullptr; + sdlController = nullptr; +} + +bool JoystickSDL::_update(void) +{ + SDL_JoystickUpdate(); + SDL_GameControllerUpdate(); + return true; +} + +bool JoystickSDL::_getButton(int i) { + if (_isGameController) { + return SDL_GameControllerGetButton(sdlController, SDL_GameControllerButton(i)) == 1; + } else { + return SDL_JoystickGetButton(sdlJoystick, i) == 1; + } +} + +int JoystickSDL::_getAxis(int i) { + if (_isGameController) { + return SDL_GameControllerGetAxis(sdlController, SDL_GameControllerAxis(i)); + } else { + return SDL_JoystickGetAxis(sdlJoystick, i); + } +} + +bool JoystickSDL::_getHat(int hat, int i) { + uint8_t hatButtons[] = {SDL_HAT_UP,SDL_HAT_DOWN,SDL_HAT_LEFT,SDL_HAT_RIGHT}; + if (i < int(sizeof(hatButtons))) { + return (SDL_JoystickGetHat(sdlJoystick, hat) & hatButtons[i]) != 0; + } + return false; +} diff --git a/src/Joystick/JoystickSDL.h b/src/Joystick/JoystickSDL.h new file mode 100644 index 0000000..b03b988 --- /dev/null +++ b/src/Joystick/JoystickSDL.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +/// @file +/// @brief SDL Joystick Interface + +#pragma once + +#include "Joystick.h" +#include "Vehicle.h" +#include "MultiVehicleManager.h" + +#include + +/// @brief SDL Joystick Interface +class JoystickSDL : public Joystick +{ +public: + JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController, MultiVehicleManager* multiVehicleManager); + + static QMap discover(MultiVehicleManager* _multiVehicleManager); + static bool init(void); + + int index(void) const { return _index; } + void setIndex(int index) { _index = index; } + + // This can be uncommented to hide the calibration buttons for gamecontrollers in the future + // bool requiresCalibration(void) final { return !_isGameController; } + +private: + static void _loadGameControllerMappings(); + + bool _open () final; + void _close () final; + bool _update () final; + + bool _getButton (int i) final; + int _getAxis (int i) final; + bool _getHat (int hat,int i) final; + + SDL_Joystick* sdlJoystick; + SDL_GameController* sdlController; + + bool _isGameController; + int _index; ///< Index for SDL_JoystickOpen + +}; diff --git a/src/PositionManager/CMakeLists.txt b/src/PositionManager/CMakeLists.txt new file mode 100644 index 0000000..b693aa5 --- /dev/null +++ b/src/PositionManager/CMakeLists.txt @@ -0,0 +1,16 @@ + +add_library(PositionManager + PositionManager.cpp + SimulatedPosition.cc +) + +target_link_libraries(PositionManager + PUBLIC + qgc +) + +target_include_directories(PositionManager + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} + ) + diff --git a/src/PositionManager/PositionManager.cpp b/src/PositionManager/PositionManager.cpp new file mode 100644 index 0000000..c668534 --- /dev/null +++ b/src/PositionManager/PositionManager.cpp @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "PositionManager.h" +#include "QGCApplication.h" +#include "QGCCorePlugin.h" + +#if !defined(NO_SERIAL_LINK) && !defined(__android__) +#include +#endif + +#include + +QGCPositionManager::QGCPositionManager(QGCApplication* app, QGCToolbox* toolbox) + : QGCTool (app, toolbox) +{ + +} + +QGCPositionManager::~QGCPositionManager() +{ + delete(_simulatedSource); + delete(_nmeaSource); +} + +void QGCPositionManager::setToolbox(QGCToolbox *toolbox) +{ + QGCTool::setToolbox(toolbox); + //-- First see if plugin provides a position source + _defaultSource = toolbox->corePlugin()->createPositionSource(this); + if (_defaultSource) { + _usingPluginSource = true; + } + + if (qgcApp()->runningUnitTests()) { + // Units test on travis fail due to lack of position source + return; + } + + if (!_defaultSource) { + //-- Otherwise, create a default one +#if 1 + // Calling this can end up falling through a path which tries to instantiate a serialnmea source. + // The Qt code for this will pop a qWarning if there are no serial ports available. This in turn + // causes you to be unable to run with QT_FATAL_WARNINGS=1 to debug stuff. + _defaultSource = QGeoPositionInfoSource::createDefaultSource(this); +#else + // So instead we create our own version of QGeoPositionInfoSource::createDefaultSource which isn't as stupid. + QList plugins = QGeoPositionInfoSourcePrivate::pluginsSorted(); + foreach (const auto &obj, plugins) { + if (obj.value("Position").isBool() && obj.value("Position").toBool()) { + QString pluginName = obj.value("Keys").toArray()[0].toString(); + if (pluginName == "serialnmea") { +#if !defined(NO_SERIAL_LINK) && !defined(__android__) + if (QSerialPortInfo::availablePorts().isEmpty()) { + // This prevents the qWarning from popping + continue; + } +#else + continue; +#endif + } + _defaultSource = QGeoPositionInfoSource::createSource(pluginName, this); + if (_defaultSource) { + break; + } + } + } +#endif + } + _simulatedSource = new SimulatedPosition(); + +#if 1 + setPositionSource(QGCPositionSource::InternalGPS); +#else + setPositionSource(QGCPositionManager::Simulated); +#endif +} + +void QGCPositionManager::setNmeaSourceDevice(QIODevice* device) +{ + // stop and release _nmeaSource + if (_nmeaSource) { + _nmeaSource->stopUpdates(); + disconnect(_nmeaSource); + + // if _currentSource is pointing there, point to null + if (_currentSource == _nmeaSource){ + _currentSource = nullptr; + } + + delete _nmeaSource; + _nmeaSource = nullptr; + + } + _nmeaSource = new QNmeaPositionInfoSource(QNmeaPositionInfoSource::RealTimeMode, this); + _nmeaSource->setDevice(device); + // set equivalent range error to enable position accuracy reporting + _nmeaSource->setUserEquivalentRangeError(5.1); + setPositionSource(QGCPositionManager::NmeaGPS); +} + +void QGCPositionManager::_positionUpdated(const QGeoPositionInfo &update) +{ + _geoPositionInfo = update; + + QGeoCoordinate newGCSPosition = QGeoCoordinate(); + qreal newGCSHeading = update.attribute(QGeoPositionInfo::Direction); + + if (update.isValid() && update.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) { + // Note that gcsPosition filters out possible crap values + if (qAbs(update.coordinate().latitude()) > 0.001 && + qAbs(update.coordinate().longitude()) > 0.001 ) { + _gcsPositionHorizontalAccuracy = update.attribute(QGeoPositionInfo::HorizontalAccuracy); + if (_gcsPositionHorizontalAccuracy <= MinHorizonalAccuracyMeters) { + newGCSPosition = update.coordinate(); + } + emit gcsPositionHorizontalAccuracyChanged(); + } + } + if (newGCSPosition != _gcsPosition) { + _gcsPosition = newGCSPosition; + emit gcsPositionChanged(_gcsPosition); + } + + // At this point only plugins support gcs heading. The reason is that the quality of heading information from a local + // position device (not a compass) is unknown. In many cases it can only be trusted if the GCS location is moving above + // a certain rate of speed. When it is not, or the gcs is standing still the heading is just random. We don't want these + // random heading to be shown on the fly view. So until we can get a true compass based heading or some smarted heading quality + // information which takes into account the speed of movement we normally don't set a heading. We do use the heading though + // if the plugin overrides the position source. In that case we assume that it hopefully know what it is doing. + if (_usingPluginSource && newGCSHeading != _gcsHeading) { + _gcsHeading = newGCSHeading; + emit gcsHeadingChanged(_gcsHeading); + } + + emit positionInfoUpdated(update); +} + +int QGCPositionManager::updateInterval() const +{ + return _updateInterval; +} + +void QGCPositionManager::setPositionSource(QGCPositionManager::QGCPositionSource source) +{ + if (_currentSource != nullptr) { + _currentSource->stopUpdates(); + disconnect(_currentSource); + + // Reset all values so we dont get stuck on old values + _geoPositionInfo = QGeoPositionInfo(); + _gcsPosition = QGeoCoordinate(); + _gcsHeading = qQNaN(); + _gcsPositionHorizontalAccuracy = std::numeric_limits::infinity(); + + emit gcsPositionChanged(_gcsPosition); + emit gcsHeadingChanged(_gcsHeading); + emit positionInfoUpdated(_geoPositionInfo); + emit gcsPositionHorizontalAccuracyChanged(); + } + + if (qgcApp()->runningUnitTests()) { + // Units test on travis fail due to lack of position source + return; + } + + switch(source) { + case QGCPositionManager::Log: + break; + case QGCPositionManager::Simulated: + _currentSource = _simulatedSource; + break; + case QGCPositionManager::NmeaGPS: + _currentSource = _nmeaSource; + break; + case QGCPositionManager::InternalGPS: + default: + _currentSource = _defaultSource; + break; + } + + if (_currentSource != nullptr) { + _updateInterval = _currentSource->minimumUpdateInterval(); + _currentSource->setPreferredPositioningMethods(QGeoPositionInfoSource::SatellitePositioningMethods); + _currentSource->setUpdateInterval(_updateInterval); + connect(_currentSource, &QGeoPositionInfoSource::positionUpdated, this, &QGCPositionManager::_positionUpdated); + connect(_currentSource, &QGeoPositionInfoSource::errorOccurred, this, &QGCPositionManager::_error); + _currentSource->startUpdates(); + } +} + +void QGCPositionManager::_error(QGeoPositionInfoSource::Error positioningError) +{ + qWarning() << "QGCPositionManager error" << positioningError; +} diff --git a/src/PositionManager/PositionManager.h b/src/PositionManager/PositionManager.h new file mode 100644 index 0000000..5654640 --- /dev/null +++ b/src/PositionManager/PositionManager.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +#include "QGCToolbox.h" +#include "SimulatedPosition.h" + +class QGCPositionManager : public QGCTool { + Q_OBJECT + +public: + static constexpr size_t MinHorizonalAccuracyMeters = 100; + QGCPositionManager(QGCApplication* app, QGCToolbox* toolbox); + ~QGCPositionManager(); + + Q_PROPERTY(QGeoCoordinate gcsPosition READ gcsPosition NOTIFY gcsPositionChanged) + Q_PROPERTY(qreal gcsHeading READ gcsHeading NOTIFY gcsHeadingChanged) + Q_PROPERTY(qreal gcsPositionHorizontalAccuracy READ gcsPositionHorizontalAccuracy + NOTIFY gcsPositionHorizontalAccuracyChanged) + + enum QGCPositionSource { + Simulated, + InternalGPS, + Log, + NmeaGPS + }; + + QGeoCoordinate gcsPosition (void) { return _gcsPosition; } + qreal gcsHeading (void) const{ return _gcsHeading; } + qreal gcsPositionHorizontalAccuracy(void) const { return _gcsPositionHorizontalAccuracy; } + QGeoPositionInfo geoPositionInfo (void) const { return _geoPositionInfo; } + void setPositionSource (QGCPositionSource source); + int updateInterval (void) const; + void setNmeaSourceDevice (QIODevice* device); + + // Overrides from QGCTool + void setToolbox(QGCToolbox* toolbox) override; + + +private slots: + void _positionUpdated(const QGeoPositionInfo &update); + void _error(QGeoPositionInfoSource::Error positioningError); + +signals: + void gcsPositionChanged(QGeoCoordinate gcsPosition); + void gcsHeadingChanged(qreal gcsHeading); + void positionInfoUpdated(QGeoPositionInfo update); + void gcsPositionHorizontalAccuracyChanged(); + +private: + int _updateInterval = 0; + QGeoPositionInfo _geoPositionInfo; + QGeoCoordinate _gcsPosition; + qreal _gcsHeading = qQNaN(); + qreal _gcsPositionHorizontalAccuracy = std::numeric_limits::infinity(); + + QGeoPositionInfoSource* _currentSource = nullptr; + QGeoPositionInfoSource* _defaultSource = nullptr; + QNmeaPositionInfoSource* _nmeaSource = nullptr; + QGeoPositionInfoSource* _simulatedSource = nullptr; + bool _usingPluginSource = false; +}; diff --git a/src/PositionManager/SimulatedPosition.cc b/src/PositionManager/SimulatedPosition.cc new file mode 100644 index 0000000..3ff6e0d --- /dev/null +++ b/src/PositionManager/SimulatedPosition.cc @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include +#include +#include + +#include "SimulatedPosition.h" +#include "QGCApplication.h" +#include "MultiVehicleManager.h" + +SimulatedPosition::SimulatedPosition() + : QGeoPositionInfoSource(nullptr) +{ + _updateTimer.setSingleShot(false); + + // Initialize position to normal PX4 Gazebo home position + _lastPosition.setTimestamp(QDateTime::currentDateTime()); + _lastPosition.setCoordinate(QGeoCoordinate(47.3977420, 8.5455941, 488)); + _lastPosition.setAttribute(QGeoPositionInfo::Attribute::Direction, _heading); + _lastPosition.setAttribute(QGeoPositionInfo::Attribute::GroundSpeed, _horizontalVelocityMetersPerSec); + _lastPosition.setAttribute(QGeoPositionInfo::Attribute::VerticalSpeed, _verticalVelocityMetersPerSec); + + // When a vehicle shows up we switch location to the vehicle home position + connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &SimulatedPosition::_vehicleAdded); + + connect(&_updateTimer, &QTimer::timeout, this, &SimulatedPosition::_updatePosition); +} + +QGeoPositionInfo SimulatedPosition::lastKnownPosition(bool /*fromSatellitePositioningMethodsOnly*/) const +{ + return _lastPosition; +} + +SimulatedPosition::PositioningMethods SimulatedPosition::supportedPositioningMethods() const +{ + return AllPositioningMethods; +} + +void SimulatedPosition::startUpdates(void) +{ + _updateTimer.start(qMax(updateInterval(), minimumUpdateInterval())); +} + +void SimulatedPosition::stopUpdates(void) +{ + _updateTimer.stop(); +} + +void SimulatedPosition::requestUpdate(int /*timeout*/) +{ + emit errorOccurred(QGeoPositionInfoSource::UpdateTimeoutError); +} + +void SimulatedPosition::_updatePosition(void) +{ + int intervalMsecs = _updateTimer.interval(); + + QGeoCoordinate coord = _lastPosition.coordinate(); + double horizontalDistance = _horizontalVelocityMetersPerSec * (1000.0 / static_cast(intervalMsecs)); + double verticalDistance = _verticalVelocityMetersPerSec * (1000.0 / static_cast(intervalMsecs)); + + _lastPosition.setCoordinate(coord.atDistanceAndAzimuth(horizontalDistance, _heading, verticalDistance)); + + emit positionUpdated(_lastPosition); +} + +QGeoPositionInfoSource::Error SimulatedPosition::error() const +{ + return QGeoPositionInfoSource::NoError; +} + +void SimulatedPosition::_vehicleAdded(Vehicle* vehicle) +{ + if (vehicle->homePosition().isValid()) { + _lastPosition.setCoordinate(vehicle->homePosition()); + } else { + connect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged); + } +} + +void SimulatedPosition::_vehicleHomePositionChanged(QGeoCoordinate homePosition) +{ + Vehicle* vehicle = qobject_cast(sender()); + + if (homePosition.isValid()) { + _lastPosition.setCoordinate(homePosition); + disconnect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged); + } +} diff --git a/src/PositionManager/SimulatedPosition.h b/src/PositionManager/SimulatedPosition.h new file mode 100644 index 0000000..357dd69 --- /dev/null +++ b/src/PositionManager/SimulatedPosition.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include "QGCToolbox.h" +#include + +class Vehicle; + +class SimulatedPosition : public QGeoPositionInfoSource +{ + Q_OBJECT + +public: + SimulatedPosition(); + + QGeoPositionInfo lastKnownPosition(bool fromSatellitePositioningMethodsOnly = false) const override; + + PositioningMethods supportedPositioningMethods (void) const override; + int minimumUpdateInterval (void) const override { return _updateIntervalMsecs; } + Error error (void) const override; + +public slots: + void startUpdates (void) override; + void stopUpdates (void) override; + void requestUpdate (int timeout = 5000) override; + +private slots: + void _updatePosition (void); + void _vehicleAdded (Vehicle* vehicle); + void _vehicleHomePositionChanged (QGeoCoordinate homePosition); + +private: + QTimer _updateTimer; + QGeoPositionInfo _lastPosition; + + static constexpr int _updateIntervalMsecs = 1000; + static constexpr double _horizontalVelocityMetersPerSec = 0.5; + static constexpr double _verticalVelocityMetersPerSec = 0.1; + static constexpr double _heading = 45; +};