diff --git a/CMakeLists.txt b/CMakeLists.txt index 065f970..c2986df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,7 +67,7 @@ include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/IOs/serial/include ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/FIFO ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G1 - ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G2 + ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/AT10 ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/Q10f ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl @@ -76,6 +76,7 @@ include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/sot/ocv470 ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/color_line ${CMAKE_CURRENT_SOURCE_DIR}/video_io + ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/veri/cuda ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/ellipse_det ${CMAKE_CURRENT_SOURCE_DIR}/utils ) @@ -116,6 +117,7 @@ set( include/sv_sot.h include/sv_mot.h include/sv_color_line.h + include/sv_veri_det.h include/sv_video_input.h include/sv_video_output.h include/sv_world.h @@ -129,11 +131,11 @@ list(APPEND serial_SRCS gimbal_ctrl/IOs/serial/src/impl/unix.cc) list(APPEND serial_SRCS gimbal_ctrl/IOs/serial/src/impl/list_ports/list_ports_linux.cc) set(driver_SRCS - gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cc + gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cpp ) file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G1/*.cpp) list(APPEND driver_SRCS ${DRV_LIB_FILES}) -file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G2/*.cpp) +file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/AT10/*.cpp) list(APPEND driver_SRCS ${DRV_LIB_FILES}) file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/Q10f/*.cpp) list(APPEND driver_SRCS ${DRV_LIB_FILES}) @@ -155,6 +157,7 @@ set(spirecv_SRCS algorithm/ellipse_det/ellipse_detector.cpp algorithm/common_det/sv_common_det.cpp algorithm/landing_det/sv_landing_det.cpp + algorithm/veri/sv_veri_det.cpp algorithm/sot/sv_sot.cpp algorithm/mot/sv_mot.cpp algorithm/color_line/sv_color_line.cpp @@ -177,6 +180,8 @@ if(USE_CUDA) list(APPEND spirecv_SRCS ${ALG_SRC_FILES}) file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/landing_det/cuda/*.cpp) list(APPEND spirecv_SRCS ${ALG_SRC_FILES}) + file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/veri/cuda/*.cpp) + list(APPEND spirecv_SRCS ${ALG_SRC_FILES}) endif() if(USE_FFMPEG) @@ -263,6 +268,8 @@ add_executable(UdpDetectionInfoSender samples/demo/udp_detection_info_sender.cpp target_link_libraries(UdpDetectionInfoSender sv_world) add_executable(VideoSaving samples/demo/video_saving.cpp) target_link_libraries(VideoSaving sv_world) +add_executable(VERI samples/demo/veri.cpp) +target_link_libraries(VERI sv_world) add_executable(VideoStreaming samples/demo/video_streaming.cpp) target_link_libraries(VideoStreaming sv_world) add_executable(GimbalClickedTracking samples/demo/gimbal_detection_with_clicked_tracking.cpp) @@ -271,14 +278,13 @@ add_executable(GimbalLandingMarkerDetection samples/demo/gimbal_landing_marker_d target_link_libraries(GimbalLandingMarkerDetection sv_world) add_executable(GimbalUdpDetectionInfoSender samples/demo/gimbal_udp_detection_info_sender.cpp) target_link_libraries(GimbalUdpDetectionInfoSender sv_world) -add_executable(ArucoDetectionWithSingleObjectTracking samples/demo/aruco_detection_with_single_object_tracking.cpp) -target_link_libraries(ArucoDetectionWithSingleObjectTracking sv_world) -add_executable(CarDetectionWithTracking samples/demo/car_detection_with_tracking.cpp) -target_link_libraries(CarDetectionWithTracking sv_world) add_executable(EvalFpsOnVideo samples/test/eval_fps_on_video.cpp) target_link_libraries(EvalFpsOnVideo sv_world) +add_executable(GimbalTest samples/test/gimbal_test.cpp) +target_link_libraries(GimbalTest sv_world) + add_executable(EvalModelOnCocoVal samples/test/eval_mAP_on_coco_val/eval_mAP_on_coco_val.cpp) target_link_libraries(EvalModelOnCocoVal sv_world) diff --git a/algorithm/mot/sv_mot.cpp b/algorithm/mot/sv_mot.cpp index 4e26283..5d886fd 100644 --- a/algorithm/mot/sv_mot.cpp +++ b/algorithm/mot/sv_mot.cpp @@ -1,6 +1,8 @@ #include "sv_mot.h" #include #include +#include +#include #include "gason.h" #include "sv_util.h" @@ -116,11 +118,11 @@ KalmanFilter::KalmanFilter() this->_F = MatrixXd::Identity(8, 8); for (int i=0; i<4; i++) { - this->_F(i,i+4) = 1; + this->_F(i,i+4) = 1.; //1 } this->_H = MatrixXd::Identity(4, 8); - this->_std_weight_position = 1. / 20; - this->_std_weight_vel = 1. / 160; + this->_std_weight_position = 1. / 20; //1./20 + this->_std_weight_vel = 1. / 160; //1./160 } KalmanFilter::~KalmanFilter() @@ -130,7 +132,9 @@ KalmanFilter::~KalmanFilter() pair, Matrix > KalmanFilter::initiate(Vector4d &bbox) { Matrix mean; - mean << bbox(0), bbox(1), bbox(2) / bbox(3), bbox(3), 0, 0, 0, 0; + Matrix zero_vector; + zero_vector.setZero(); + mean << bbox(0), bbox(1), (double)bbox(2) / (double)bbox(3), bbox(3), zero_vector; VectorXd stds(8); stds << 2 * this->_std_weight_position * mean(3), 2 * this->_std_weight_position * mean(3), 0.01, 2 * this->_std_weight_position * mean(3), \ 10 * this->_std_weight_vel * mean(3), 10 * this->_std_weight_vel * mean(3), 1e-5, 10 * this->_std_weight_vel * mean(3); @@ -142,31 +146,43 @@ pair, Matrix > KalmanFilter::initiate(Vector4 pair, Matrix > KalmanFilter::update(Matrix mean, Matrix covariances, sv::Box &box) { - MatrixXd R; - Vector4d stds; - - stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 0.1, this->_std_weight_position * mean(3); - MatrixXd squared = stds.array().square(); - R = squared.asDiagonal(); - MatrixXd S = this->_H * covariances * this->_H.transpose() + R; - - MatrixXd Kalman_gain = covariances * this->_H.transpose() * S.inverse(); VectorXd measurement(4); - measurement << box.x1, box.y1, (box.x2-box.x1) / (box.y2-box.y1), box.y2-box.y1; - Matrix new_mean = mean + Kalman_gain * (measurement - this->_H * mean); - Matrix new_covariances = (MatrixXd::Identity(8, 8) - Kalman_gain * this->_H) * covariances; - return make_pair(new_mean, new_covariances); + double a = (double)(box.x2-box.x1) / (double)(box.y2-box.y1); + measurement << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, a, box.y2-box.y1; + pair, Matrix > projected = project(mean, covariances); + Matrix projected_mean = projected.first; + Matrix projected_cov = projected.second; + + Eigen::LLT chol_factor(projected_cov); + MatrixXd Kalman_gain = (chol_factor.solve((covariances * this->_H.transpose()).transpose())).transpose(); + + VectorXd innovation = measurement - projected_mean; + Matrix new_mean = mean + Kalman_gain *innovation; + Matrix new_covariances = covariances - Kalman_gain * projected_cov * Kalman_gain.transpose(); + + return make_pair(new_mean, new_covariances); } +pair, Matrix > KalmanFilter::project(Matrix mean, Matrix covariances) +{ + VectorXd stds(4); + stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 0.1, this->_std_weight_position * mean(3); + MatrixXd squared = stds.array().square(); + MatrixXd R = squared.asDiagonal(); + + Matrix pro_mean = this->_H * mean; + Matrix pro_covariances = this->_H * covariances * this->_H.transpose() + R; + return make_pair(pro_mean, pro_covariances); +} pair, Matrix > KalmanFilter::predict(Matrix mean, Matrix covariances) { VectorXd stds(8); - stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 0.01, this->_std_weight_position * mean(3), \ - this->_std_weight_vel * mean(3), this->_std_weight_vel * mean(3), 1e-5, this->_std_weight_vel * mean(3); + stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 1e-2, this->_std_weight_position * mean(3), \ + this->_std_weight_vel * mean(3), this->_std_weight_vel * mean(3), 1e-5, this->_std_weight_vel * mean(3); // a = 0.01 MatrixXd squared = stds.array().square(); MatrixXd Q = squared.asDiagonal(); Matrix pre_mean = this->_F * mean; - Matrix pre_cov = this->_F * covariances * this->_F.transpose() + Q; + Matrix pre_cov = this->_F * covariances * this->_F.transpose()+Q;//+Q return make_pair(pre_mean, pre_cov); } @@ -178,7 +194,7 @@ SORT::~SORT() void SORT::update(TargetsInFrame& tgts) { sv::KalmanFilter kf; - if (! this->_tracklets.size()) + if (! this->_tracklets.size() || tgts.targets.size() == 0) { Vector4d bbox; for (int i=0; i_next_tracklet_id; - // cout << tracklet.id << endl; tgts.targets[i].tracked_id = this->_next_tracklet_id; tgts.targets[i].has_tid = true; - tracklet.bbox << box.x1, box.y1, box.x2-box.x1, box.y2-box.y1; // x,y,w,h + tracklet.bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1; // x,y,w,h; center(x,y) + tracklet.age = 0; tracklet.hits = 1; tracklet.misses = 0; tracklet.frame_id = tgts.frame_id; + tracklet.category_id = tgts.targets[i].category_id; tracklet.tentative = true; + // initate the motion pair, Matrix > motion = kf.initiate(tracklet.bbox); tracklet.mean = motion.first; tracklet.covariance = motion.second; - + this->_tracklets.push_back(tracklet); } } else { + // cout << "frame id:" << tgts.frame_id << endl; for (int i=0; i match_det; - match_det.fill(-1); + vector match_det(tgts.targets.size(), -1); // predict the next state of each tracklet for (auto& tracklet : this->_tracklets) { @@ -226,8 +244,6 @@ void SORT::update(TargetsInFrame& tgts) } // Match the detections to the existing tracklets - // cout << "the num of targets: " << tgts.targets.size() << endl; - // cout << "the num of tracklets: " << this->_tracklets.size() << endl; vector > iouMatrix(this->_tracklets.size(), vector (tgts.targets.size(), 0)); for (int i=0; i_tracklets.size(); i++) { @@ -238,6 +254,7 @@ void SORT::update(TargetsInFrame& tgts) iouMatrix[i][j] = this->_iou(this->_tracklets[i], box); } } + vector > matches = this->_hungarian(iouMatrix); for (auto& match : matches) { @@ -245,50 +262,58 @@ void SORT::update(TargetsInFrame& tgts) int detectionIndex = match.second; if (trackletIndex >= 0 && detectionIndex >= 0) { - if (iouMatrix[match.first][match.second] >= _iou_threshold) // iou_thrshold + if (iouMatrix[match.first][match.second] <= 1-_iou_threshold) // iou_thrshold { sv::Box box; tgts.targets[detectionIndex].getBox(box); this->_tracklets[trackletIndex].age = 0; this->_tracklets[trackletIndex].hits++; this->_tracklets[trackletIndex].frame_id = tgts.frame_id; - this->_tracklets[trackletIndex].bbox << box.x1, box.y1, box.x2-box.x1, box.y2-box.y1; - - auto[mean, covariance] = kf.update(this->_tracklets[trackletIndex].mean, this->_tracklets[trackletIndex].covariance, box); - this->_tracklets[trackletIndex].mean = mean; - this->_tracklets[trackletIndex].covariance = covariance; - + this->_tracklets[trackletIndex].bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1; tgts.targets[detectionIndex].tracked_id = this->_tracklets[trackletIndex].id; - match_det[detectionIndex] = detectionIndex; + match_det[detectionIndex] = trackletIndex; } } } - // create new tracklets for unmatched detections + std::vector > ().swap(iouMatrix); for (int i=0; i_next_tracklet_id; - tracklet.bbox << box.x1, box.y1, box.x2-box.x1, box.y2-box.y1; - + tracklet.bbox << box.x1+(box.x2-box.x1)/2, (double)(box.y1+(box.y2-box.y1)/2), box.x2-box.x1, box.y2-box.y1; tracklet.age = 0; tracklet.hits = 1; tracklet.misses = 0; tracklet.frame_id = tgts.frame_id; + tracklet.category_id = tgts.targets[i].category_id; tracklet.tentative = true; - auto[new_mean, new_covariance] = kf.initiate(tracklet.bbox); - tracklet.mean = new_mean; - tracklet.covariance = new_covariance; - + pair, Matrix > new_motion = kf.initiate(tracklet.bbox); + tracklet.mean = new_motion.first; + tracklet.covariance = new_motion.second; + tgts.targets[i].tracked_id = this->_next_tracklet_id; tgts.targets[i].has_tid = true; this->_tracklets.push_back(tracklet); } + else + { + sv::Box box; + int track_id = match_det[i]; + tgts.targets[i].getBox(box); + pair, Matrix > updated = kf.update(this->_tracklets[track_id].mean, this->_tracklets[track_id].covariance, box); + this->_tracklets[track_id].mean = updated.first; + this->_tracklets[track_id].covariance = updated.second; + } } + + //sift tracklets for (auto& tracklet : this->_tracklets) { if (tracklet.hits >= _min_hits) @@ -312,15 +337,16 @@ vector SORT::getTracklets() const double SORT::_iou(Tracklet& tracklet, sv::Box& box) { - double trackletX1 = tracklet.bbox(0); - double trackletY1 = tracklet.bbox(1); - double trackletX2 = tracklet.bbox(0) + tracklet.bbox(2); - double trackletY2 = tracklet.bbox(1) + tracklet.bbox(3); - + double trackletX1 = tracklet.bbox(0)-tracklet.bbox(2)/2; + double trackletY1 = tracklet.bbox(1)-tracklet.bbox(3)/2; + double trackletX2 = tracklet.bbox(0) + tracklet.bbox(2)/2; + double trackletY2 = tracklet.bbox(1) + tracklet.bbox(3)/2; + double detectionX1 = box.x1; double detectionY1 = box.y1; double detectionX2 = box.x2; double detectionY2 = box.y2; + double intersectionX1 = max(trackletX1, detectionX1); double intersectionY1 = max(trackletY1, detectionY1); double intersectionX2 = min(trackletX2, detectionX2); @@ -339,19 +365,55 @@ double SORT::_iou(Tracklet& tracklet, sv::Box& box) return iou; } +// Function to find the minimum element in a vector +double SORT::_findMin(const std::vector& vec) { + double minVal = std::numeric_limits::max(); + for (double val : vec) { + if (val < minVal) { + minVal = val; + } + } + return minVal; +} + +// Function to subtract the minimum value from each row of the cost matrix +void SORT::_subtractMinFromRows(std::vector>& costMatrix) { + for (auto& row : costMatrix) { + double minVal = _findMin(row); + for (double& val : row) { + val -= minVal; + } + } +} + +// Function to subtract the minimum value from each column of the cost matrix +void SORT::_subtractMinFromCols(std::vector>& costMatrix) { + for (size_t col = 0; col < costMatrix[0].size(); ++col) { + double minVal = std::numeric_limits::max(); + for (size_t row = 0; row < costMatrix.size(); ++row) { + if (costMatrix[row][col] < minVal) { + minVal = costMatrix[row][col]; + } + } + for (size_t row = 0; row < costMatrix.size(); ++row) { + costMatrix[row][col] -= minVal; + } + } +} + +// Function to find a matching using the Hungarian algorithm vector > SORT::_hungarian(vector > costMatrix) { - int numRows = costMatrix.size(); - int numCols = costMatrix[0].size(); + size_t numRows = costMatrix.size(); + size_t numCols = costMatrix[0].size(); + //transpose the matrix if necessary const bool transposed = numCols > numRows; - // transpose the matrix if necessary - if (transposed) - { - vector > transposedMatrix(numCols, vector(numRows)); - for (int i=0; i> transposedMatrix(numCols, vector(numRows)); + for (int i = 0; i < numRows; i++) { - for (int j=0; j > SORT::_hungarian(vector > costMatrix) costMatrix = transposedMatrix; swap(numRows, numCols); } - vectorrowMin(numRows, numeric_limits::infinity()); - vectorcolMin(numCols, numeric_limits::infinity()); - vectorrowMatch(numRows, -1); - vectorcolMatch(numCols, -1); - vector > matches; - // step1: Subtract the row minimums from each row - for (int i=0; i visited(numCols, false); - this->_augment(costMatrix, i, rowMatch, colMatch, visited); - } - // step4: calculate the matches - matches.clear(); - for (int j=0; j >& costMatrix, int row, vector& rowMatch, vector& colMatch, vector& visited) -{ - int numCols = costMatrix[0].size(); - for (int j=0; j_augment(costMatrix, colMatch[j], rowMatch, colMatch, visited)) - { - rowMatch[row] = j; - colMatch[j] = row; - return true; + // Create a square cost matrix by padding with zeros if necessary + std::vector> squareMatrix(maxDim, std::vector(maxDim, 0.0)); + for (size_t row = 0; row < numRows; ++row) { + for (size_t col = 0; col < numCols; ++col) { + squareMatrix[row][col] = costMatrix[row][col]; + } + } + + // Subtract the minimum value from each row and column + _subtractMinFromRows(squareMatrix); + _subtractMinFromCols(squareMatrix); + + // Initialize the assignment vectors with -1 values + std::vector rowAssignment(maxDim, -1); + std::vector colAssignment(maxDim, -1); + + // Perform the matching + for (size_t row = 0; row < maxDim; ++row) { + std::vector visitedCols(maxDim, false); + for (size_t col = 0; col < maxDim; ++col) { + if (squareMatrix[row][col] == 0 && colAssignment[col] == -1) { + rowAssignment[row] = col; + colAssignment[col] = row; + break; } } } - return false; -} - - + + // Convert the assignment vectors to pair format + std::vector> assignmentPairs; + for (size_t row = 0; row < numRows; ++row) { + int col = rowAssignment[row]; + //if (col != -1) { + // assignmentPairs.emplace_back(row, col); + // } + if (col != -1) { + if (col >= numCols) { + col = -1; + } + assignmentPairs.emplace_back(row, col); + } + } + if (transposed) { + for (auto& assignment : assignmentPairs) + { + swap(assignment.first, assignment.second); + } + } + return assignmentPairs; + } } diff --git a/algorithm/sot/ocv470/sot_ocv470_impl.cpp b/algorithm/sot/ocv470/sot_ocv470_impl.cpp index b711f2e..2836eb8 100644 --- a/algorithm/sot/ocv470/sot_ocv470_impl.cpp +++ b/algorithm/sot/ocv470/sot_ocv470_impl.cpp @@ -39,8 +39,8 @@ bool SingleObjectTrackerOCV470Impl::ocv470Setup(SingleObjectTrackerBase* base_) TrackerNano::Params nano_params; nano_params.backbone = samples::findFile(backbone); nano_params.neckhead = samples::findFile(neckhead); - nano_params.backend = this->_backend; - nano_params.target = this->_target; + nano_params.backend = cv::dnn::DNN_BACKEND_CUDA; + nano_params.target = cv::dnn::DNN_TARGET_CUDA; _nano = TrackerNano::create(nano_params); } diff --git a/algorithm/sv_algorithm_base.cpp b/algorithm/sv_algorithm_base.cpp index 3a298b1..0633286 100644 --- a/algorithm/sv_algorithm_base.cpp +++ b/algorithm/sv_algorithm_base.cpp @@ -1248,3 +1248,4 @@ namespace sv } } + diff --git a/algorithm/veri/cuda/veri_det_cuda_impl.cpp b/algorithm/veri/cuda/veri_det_cuda_impl.cpp new file mode 100644 index 0000000..d5225ec --- /dev/null +++ b/algorithm/veri/cuda/veri_det_cuda_impl.cpp @@ -0,0 +1,189 @@ +#include "veri_det_cuda_impl.h" +#include +#include + +#define SV_MODEL_DIR "/SpireCV/models/" +#define SV_ROOT_DIR "/SpireCV/" + +#ifdef WITH_CUDA +#include "yolov7/logging.h" +#define TRTCHECK(status) \ + do \ + { \ + auto ret = (status); \ + if (ret != 0) \ + { \ + std::cerr << "Cuda failure: " << ret << std::endl; \ + abort(); \ + } \ + } while (0) + +#define DEVICE 0 // GPU id +#define BATCH_SIZE 1 + +#define MAX_IMAGE_INPUT_SIZE_THRESH 3000 * 3000 // ensure it exceed the maximum size in the input images ! +#endif + +#include +#include +int BAT = 1; +float cosineSimilarity(float *vec1, float *vec2, int size) +{ + // 计算向量的点积 + float dotProduct = 0.0f; + for (int i = 0; i < size; ++i) + { + dotProduct += vec1[i] * vec2[i]; + } + + // 计算向量的模长 + float magnitudeVec1 = 0.0f; + float magnitudeVec2 = 0.0f; + for (int i = 0; i < size; ++i) + { + magnitudeVec1 += vec1[i] * vec1[i]; + magnitudeVec2 += vec2[i] * vec2[i]; + } + magnitudeVec1 = std::sqrt(magnitudeVec1); + magnitudeVec2 = std::sqrt(magnitudeVec2); + + // 计算余弦相似性 + float similarity = dotProduct / (magnitudeVec1 * magnitudeVec2); + + return similarity; +} + +namespace sv +{ + + using namespace cv; + +#ifdef WITH_CUDA + using namespace nvinfer1; + static Logger g_nvlogger; +#endif + + VeriDetectorCUDAImpl::VeriDetectorCUDAImpl() + { + } + + VeriDetectorCUDAImpl::~VeriDetectorCUDAImpl() + { + } + + bool VeriDetectorCUDAImpl::cudaSetup() + { +#ifdef WITH_CUDA + std::string trt_model_fn = get_home() + SV_MODEL_DIR + "model.engine"; + if (!is_file_exist(trt_model_fn)) + { + throw std::runtime_error("SpireCV (104) Error loading the VERI TensorRT model (File Not Exist)"); + } + char *trt_model_stream{nullptr}; + size_t trt_model_size{0}; + try + { + std::ifstream file(trt_model_fn, std::ios::binary); + file.seekg(0, file.end); + trt_model_size = file.tellg(); + file.seekg(0, file.beg); + trt_model_stream = new char[trt_model_size]; + assert(trt_model_stream); + file.read(trt_model_stream, trt_model_size); + file.close(); + } + catch (const std::runtime_error &e) + { + throw std::runtime_error("SpireCV (104) Error loading the TensorRT model!"); + } + + // TensorRT + IRuntime *runtime = nvinfer1::createInferRuntime(g_nvlogger); + assert(runtime != nullptr); + ICudaEngine *p_cu_engine = runtime->deserializeCudaEngine(trt_model_stream, trt_model_size); + assert(p_cu_engine != nullptr); + this->_trt_context = p_cu_engine->createExecutionContext(); + assert(this->_trt_context != nullptr); + + delete[] trt_model_stream; + const ICudaEngine &cu_engine = this->_trt_context->getEngine(); + assert(cu_engine.getNbBindings() == 2); + + this->_input_index = cu_engine.getBindingIndex("input"); + this->_output_index1 = cu_engine.getBindingIndex("output"); + this->_output_index2 = cu_engine.getBindingIndex("/head/layers.0/act/Mul_output_0"); + TRTCHECK(cudaMalloc(&_p_buffers[this->_input_index], 2 * 3 * 224 * 224 * sizeof(float))); + TRTCHECK(cudaMalloc(&_p_buffers[this->_output_index1], 2 * 576 * sizeof(float))); + TRTCHECK(cudaMalloc(&_p_buffers[this->_output_index2], 2 * 1280 * sizeof(float))); + TRTCHECK(cudaStreamCreate(&_cu_stream)); + + auto input_dims = nvinfer1::Dims4{2, 3, 224, 224}; + this->_trt_context->setBindingDimensions(this->_input_index, input_dims); + + this->_p_data = new float[2 * 3 * 224 * 224]; + this->_p_prob1 = new float[2 * 576]; + this->_p_prob2 = new float[2 * 1280]; + this->_p_prob3 = new float[2 * 1280]; + // Input + TRTCHECK(cudaMemcpyAsync(_p_buffers[this->_input_index], this->_p_data, 2 * 3 * 224 * 224 * sizeof(float), cudaMemcpyHostToDevice, this->_cu_stream)); + // this->_trt_context->enqueue(1, _p_buffers, this->_cu_stream, nullptr); + this->_trt_context->enqueueV2(_p_buffers, this->_cu_stream, nullptr); + // Output + TRTCHECK(cudaMemcpyAsync(this->_p_prob1, _p_buffers[this->_output_index1], 2 * 576 * sizeof(float), cudaMemcpyDeviceToHost, this->_cu_stream)); + TRTCHECK(cudaMemcpyAsync(this->_p_prob2, _p_buffers[this->_output_index2], 2 * 1280 * sizeof(float), cudaMemcpyDeviceToHost, this->_cu_stream)); + cudaStreamSynchronize(this->_cu_stream); + return true; +#endif + return false; + } + + void VeriDetectorCUDAImpl::cudaRoiCNN( + std::vector &input_rois_, + std::vector &output_labels_) + { +#ifdef WITH_CUDA + + for (int i = 0; i < 2; ++i) + { + for (int row = 0; row < 224; ++row) + { + uchar *uc_pixel = input_rois_[i].data + row * input_rois_[i].step; // compute row id + for (int col = 0; col < 224; ++col) + { + // mean=[136.20, 141.50, 145.41], std=[44.77, 44.20, 44.30] + this->_p_data[224 * 224 * 3 * i + col + row * 224] = ((float)uc_pixel[0] - 136.20f) / 44.77f; + this->_p_data[224 * 224 * 3 * i + col + row * 224 + 224 * 224] = ((float)uc_pixel[1] - 141.50f) / 44.20f; + this->_p_data[224 * 224 * 3 * i + col + row * 224 + 224 * 224 * 2] = ((float)uc_pixel[2] - 145.41f) / 44.30f; + uc_pixel += 3; + } + } + } + + // Input + TRTCHECK(cudaMemcpyAsync(_p_buffers[this->_input_index], this->_p_data, 2 * 3 * 224 * 224 * sizeof(float), cudaMemcpyHostToDevice, this->_cu_stream)); + // this->_trt_context->enqueue(1, _p_buffers, this->_cu_stream, nullptr); + this->_trt_context->enqueueV2(_p_buffers, this->_cu_stream, nullptr); + // Output + TRTCHECK(cudaMemcpyAsync(this->_p_prob1, _p_buffers[this->_output_index1], 2 * 576 * sizeof(float), cudaMemcpyDeviceToHost, this->_cu_stream)); + TRTCHECK(cudaMemcpyAsync(this->_p_prob2, _p_buffers[this->_output_index2], 2 * 1280 * sizeof(float), cudaMemcpyDeviceToHost, this->_cu_stream)); + cudaStreamSynchronize(this->_cu_stream); + + // Find max index + double max = 0; + int label = 0; + for (int i = 0; i < 576; ++i) + { + if (max < this->_p_prob1[i]) + { + max = this->_p_prob1[i]; + label = i; + } + } + + // 计算两个数组的余弦相似性 + float similarity = cosineSimilarity(_p_prob2, _p_prob2 + 1280, 1280); + std::cout << "余弦相似性: " << similarity << std::endl; + std::cout << "VERI LABEL: " << label << std::endl; + } +#endif +} diff --git a/algorithm/veri/cuda/veri_det_cuda_impl.h b/algorithm/veri/cuda/veri_det_cuda_impl.h new file mode 100644 index 0000000..559a67f --- /dev/null +++ b/algorithm/veri/cuda/veri_det_cuda_impl.h @@ -0,0 +1,51 @@ +#ifndef __SV_VERI_DET_CUDA__ +#define __SV_VERI_DET_CUDA__ + +#include "sv_core.h" +#include +#include +#include +#include +#include + + + +#ifdef WITH_CUDA +#include +#include +#endif + + + +namespace sv { + + +class VeriDetectorCUDAImpl +{ +public: + VeriDetectorCUDAImpl(); + ~VeriDetectorCUDAImpl(); + + bool cudaSetup(); + void cudaRoiCNN( + std::vector& input_rois_, + std::vector& output_labels_ + ); + +#ifdef WITH_CUDA + float *_p_data; + float *_p_prob1; + float *_p_prob2; + float *_p_prob3; + nvinfer1::IExecutionContext *_trt_context; + int _input_index; + int _output_index1; + int _output_index2; + void *_p_buffers[3]; + cudaStream_t _cu_stream; +#endif +}; + + +} +#endif diff --git a/algorithm/veri/sv_veri_det.cpp b/algorithm/veri/sv_veri_det.cpp new file mode 100644 index 0000000..a7df6d7 --- /dev/null +++ b/algorithm/veri/sv_veri_det.cpp @@ -0,0 +1,61 @@ +#include "sv_veri_det.h" +#include +#include +#ifdef WITH_CUDA +#include +#include +#include "veri_det_cuda_impl.h" +#endif + + +namespace sv { + + +VeriDetector::VeriDetector() +{ + this->_cuda_impl = new VeriDetectorCUDAImpl; +} +VeriDetector::~VeriDetector() +{ +} + +bool VeriDetector::setupImpl() +{ +#ifdef WITH_CUDA + return this->_cuda_impl->cudaSetup(); +#endif + return false; +} + +void VeriDetector::roiCNN( + std::vector& input_rois_, + std::vector& output_labels_ +) +{ +#ifdef WITH_CUDA + this->_cuda_impl->cudaRoiCNN( + input_rois_, + output_labels_ + ); +#endif +} + + + +void VeriDetector::detect(cv::Mat img1_, cv::Mat img2_, TargetsInFrame& tgts_) +{ + if (!_params_loaded) + { + this->_load(); + this->_loadLabels(); + _params_loaded = true; + } + + std::vector e_roi = {img1_, img2_}; + + std::vector output_labels; + roiCNN(e_roi, output_labels); +} + +} + diff --git a/calib_webcam_1280x720.yaml b/calib_webcam_1280x720.yaml index 39df5ce..bf3fb71 100644 --- a/calib_webcam_1280x720.yaml +++ b/calib_webcam_1280x720.yaml @@ -8,13 +8,10 @@ camera_matrix: !!opencv-matrix rows: 3 cols: 3 dt: d - data: [ 7.9379415710551370e+02, 0., 2.9783879354295328e+02, 0., - 7.9491985564466654e+02, 3.0942416136837386e+02, 0., 0., 1. ] + data: [ 919.698486328125, 0.0, 638.8856201171875, 0.0, 920.1767578125, 370.9995422363281, 0.0, 0.0, 1.0] distortion_coefficients: !!opencv-matrix rows: 1 cols: 5 dt: d - data: [ 2.0950200339181715e-01, -1.1587468096518483e+00, - 5.5342063671841328e-03, 2.2214393775334758e-04, - 1.7127431916651392e+00 ] -avg_reprojection_error: 2.8342964851391211e-01 + data: [ 0, 0, 0, 0, 0] +avg_reprojection_error: 0 diff --git a/calib_webcam_1920x1080.yaml b/calib_webcam_1920x1080.yaml new file mode 100644 index 0000000..b1fc27d --- /dev/null +++ b/calib_webcam_1920x1080.yaml @@ -0,0 +1,20 @@ +%YAML:1.0 +--- +calibration_time: "2021年01月12日 星期二 18时08分01秒" +image_width: 1920 +image_height: 1080 +flags: 0 +camera_matrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 7.9379415710551370e+02, 0., 2.9783879354295328e+02, 0., + 7.9491985564466654e+02, 3.0942416136837386e+02, 0., 0., 1. ] +distortion_coefficients: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ 2.0950200339181715e-01, -1.1587468096518483e+00, + 5.5342063671841328e-03, 2.2214393775334758e-04, + 1.7127431916651392e+00 ] +avg_reprojection_error: 2.8342964851391211e-01 diff --git a/calib_webcam_640x480.yaml b/calib_webcam_640x480.yaml index 54f1526..abd8ab0 100644 --- a/calib_webcam_640x480.yaml +++ b/calib_webcam_640x480.yaml @@ -1,6 +1,6 @@ %YAML:1.0 --- -calibration_time: "2023年07月14日 星期五 16时39分17秒" +calibration_time: "2021年01月12日 星期二 18时08分01秒" image_width: 640 image_height: 480 flags: 0 @@ -8,13 +8,13 @@ camera_matrix: !!opencv-matrix rows: 3 cols: 3 dt: d - data: [ 4.5099311307542973e+02, 0., 3.2898947972890943e+02, 0., - 6.0215873600107579e+02, 2.4195307609106428e+02, 0., 0., 1. ] + data: [ 7.9379415710551370e+02, 0., 2.9783879354295328e+02, 0., + 7.9491985564466654e+02, 3.0942416136837386e+02, 0., 0., 1. ] distortion_coefficients: !!opencv-matrix rows: 1 cols: 5 dt: d - data: [ 1.0737258446369682e-01, -1.2782122264046064e-01, - 1.6844258609297487e-03, -6.6256775118868144e-04, - -3.5333889479158398e-01 ] -avg_reprojection_error: 3.3968000452388564e-01 + data: [ 2.0950200339181715e-01, -1.1587468096518483e+00, + 5.5342063671841328e-03, 2.2214393775334758e-04, + 1.7127431916651392e+00 ] +avg_reprojection_error: 2.8342964851391211e-01 diff --git a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h new file mode 100755 index 0000000..9a9ce3e --- /dev/null +++ b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h @@ -0,0 +1,38 @@ +/* + * @Description: + * @Author: L LC @amov + * @Date: 2022-10-27 18:10:06 + * @LastEditors: L LC @amov + * @LastEditTime: 2023-08-16 21:53:43 + * @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_crc32.h + */ +#ifndef AT10_GIMBAL_CRC32_H +#define AT10_GIMBAL_CRC32_H + +namespace AT10 +{ + static inline unsigned char CheckSum(unsigned char *pData, unsigned short Lenght) + { + unsigned short temp = 0; + unsigned short i = 0; + for (i = 0; i < Lenght; i++) + { + temp += pData[i]; + } + return temp & 0XFF; + } + + static inline unsigned char checkXOR(unsigned char *pData, unsigned char Lenght) + { + unsigned char temp = Lenght; + unsigned char i; + for (i = 1; i < Lenght - 1; i++) + { + temp ^= pData[i]; + } + return temp; + } + +} // namespace name + +#endif \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp new file mode 100755 index 0000000..ca8263f --- /dev/null +++ b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp @@ -0,0 +1,425 @@ +/* + * @Description: + * @Author: L LC @amov + * @Date: 2022-10-27 18:10:06 + * @LastEditors: L LC @amov + * @LastEditTime: 2023-08-25 19:39:56 + * @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_driver.cpp + */ +#include "AT10_gimbal_driver.h" +#include "AT10_gimbal_crc32.h" +#include "string.h" + +/** + * The function creates a new instance of the g1GimbalDriver class, which is a subclass of the + * IamovGimbalBase class + * + * @param _IO The IOStreamBase object that will be used to communicate with the gimbal. + */ +AT10GimbalDriver::AT10GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::amovGimbalBase(_IO) +{ + rxQueue = new fifoRing(sizeof(AT10::GIMBAL_EXTEND_FRAME_T), MAX_QUEUE_SIZE); + txQueue = new fifoRing(sizeof(AT10::GIMBAL_EXTEND_FRAME_T), MAX_QUEUE_SIZE); + + stdRxQueue = new fifoRing(sizeof(AT10::GIMBAL_STD_FRAME_T), MAX_QUEUE_SIZE); + stdTxQueue = new fifoRing(sizeof(AT10::GIMBAL_STD_FRAME_T), MAX_QUEUE_SIZE); + parserState = AT10::GIMBAL_SERIAL_STATE_IDLE; + + // Initialize and enable attitude data return (50Hz) + // uint8_t cmd = 0XFF; + // pack(AT10::GIMBAL_CMD_SET_FEEDBACK_H, &cmd, 1); + // pack(AT10::GIMBAL_CMD_SET_FEEDBACK_L, &cmd, 1); + // cmd = 0X00; + // pack(AT10::GIMBAL_CMD_OPEN_FEEDBACK, &cmd, 1); +} + +/** + * The function takes a command, a pointer to a payload, and the size of the payload. It then copies + * the payload into the tx buffer, calculates the checksum, and then calculates the CRC32 of the + * payload. It then copies the CRC32 into the tx buffer, and then copies the tx buffer into the txQueue + * + * @param uint32_t 4 bytes + * @param pPayload pointer to the data to be sent + * @param payloadSize the size of the payload + * + * @return The size of the data to be sent. + */ +uint32_t AT10GimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize) +{ + uint32_t ret = 0; + + if (cmd > 0XFF) + { + AT10::GIMBAL_EXTEND_FRAME_T txTemp; + txTemp.head = cmd; + memcpy(txTemp.data, pPayload, payloadSize); + payloadSize--; + txTemp.len = payloadSize; + if (txQueue->inCell(&txTemp)) + { + ret = payloadSize + sizeof(uint32_t) + sizeof(uint8_t); + } + } + else + { + AT10::GIMBAL_STD_FRAME_T txTemp; + txTemp.head = AT10::GIMBAL_CMD_STD; + txTemp.len = payloadSize + 3; + txTemp.cmd = cmd; + memcpy(txTemp.data, pPayload, payloadSize); + txTemp.data[payloadSize] = AT10::checkXOR((uint8_t *)&txTemp.len, txTemp.len); + if (stdTxQueue->inCell(&txTemp)) + { + ret = payloadSize + 6; + } + } + + return ret; +} + +void AT10GimbalDriver::convert(void *buf) +{ + AT10::GIMBAL_EXTEND_FRAME_T *temp; + temp = reinterpret_cast(buf); + switch (temp->head) + { + case AT10::GIMBAL_CMD_RCV_STATE: + std::cout << "Undefined old frame from AT10"; + // AT10::GIMBAL_RCV_POS_MSG_T *tempPos; + // tempPos = reinterpret_cast(((uint8_t *)buf) + AT10_EXT_PAYLOAD_OFFSET); + // mState.lock(); + // state.abs.yaw = tempPos->yawIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE; + // state.abs.roll = tempPos->rollIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE; + // state.abs.pitch = tempPos->pitchIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE; + // state.rel.yaw = tempPos->yawStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED; + // state.rel.roll = tempPos->rollStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED; + // state.rel.pitch = tempPos->pitchStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED; + // updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, + // state.abs.roll, state.abs.pitch, state.abs.yaw, + // state.fov.x, state.fov.y); + // mState.unlock(); + + break; + + case AT10::GIMBAL_CMD_STD: + AT10::GIMBAL_STD_FRAME_T *stdTemp; + stdTemp = reinterpret_cast(buf); + switch (stdTemp->cmd) + { + case AT10::GIMBAL_CMD_STD_RCV_STATE: + AT10::GIMBAL_RCV_STD_STATE_MSG_T *tempRcv; + tempRcv = reinterpret_cast(((uint8_t *)buf) + AT10_STD_PAYLOAD_OFFSET); + mState.lock(); + + state.abs.roll = (amovGimbalTools::conversionBigLittle((uint16_t)(tempRcv->B1.roll & 0XFF0F)) * 0.043956043956044f) - 90.0f; + state.abs.yaw = (int16_t)amovGimbalTools::conversionBigLittle((uint16_t)tempRcv->B1.yaw) * 0.0054931640625f; + state.abs.pitch = (int16_t)amovGimbalTools::conversionBigLittle((uint16_t)tempRcv->B1.pitch) * 0.0054931640625f; + + state.rel.yaw = state.abs.yaw; + state.rel.roll = state.abs.roll; + state.rel.pitch = state.abs.pitch; + + state.fov.x = amovGimbalTools::conversionBigLittle(tempRcv->D1.fovX) * 0.1; + state.fov.y = amovGimbalTools::conversionBigLittle(tempRcv->D1.fovY) * 0.1; + if ((amovGimbalTools::conversionBigLittle(tempRcv->D1.camera) & 0X0003) == 0X01) + { + state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE; + } + else + { + state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF; + } + updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, + state.abs.roll, state.abs.pitch, state.abs.yaw, + state.fov.x, state.fov.y); + mState.unlock(); + break; + + case AT10::GIMBAL_CMD_STD_NOP: + break; + + default: + std::cout << "Undefined std frame from AT10"; + std::cout << std::endl; + break; + } + break; + default: + printf("\r\nUndefined frame from AT10,head:%08X", temp->head); + break; + } +} + +/** + * It's a state machine that parses a serial stream of bytes into a struct + * + * @param uint8_t unsigned char + * + * @return A boolean value. + */ +bool AT10GimbalDriver::parser(IN uint8_t byte) +{ + bool state = false; + static uint8_t payloadLenghte = 0; + static uint8_t *pRx = nullptr; + uint8_t suncheck; + + switch (parserState) + { + case AT10::GIMBAL_SERIAL_STATE_IDLE: + if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0X000000FF) >> 0)) + { + parserState = AT10::GIMBAL_SERIAL_STATE_EXT_HEAD1; + } + else if (byte == ((AT10::GIMBAL_CMD_STD & 0X0000FF00) >> 8)) + { + parserState = AT10::GIMBAL_SERIAL_STATE_STD_HAED1; + } + break; + + // STD msg + case AT10::GIMBAL_SERIAL_STATE_STD_HAED1: + if (byte == ((AT10::GIMBAL_CMD_STD & 0X00FF0000) >> 16)) + { + parserState = AT10::GIMBAL_SERIAL_STATE_STD_HAED2; + } + else + { + parserState = AT10::GIMBAL_SERIAL_STATE_IDLE; + } + break; + + case AT10::GIMBAL_SERIAL_STATE_STD_HAED2: + if (byte == ((AT10::GIMBAL_CMD_STD & 0XFF000000) >> 24)) + { + parserState = AT10::GIMBAL_SERIAL_STATE_STD_LEN; + } + else + { + parserState = AT10::GIMBAL_SERIAL_STATE_IDLE; + } + break; + + case AT10::GIMBAL_SERIAL_STATE_STD_LEN: + stdRx.len = byte; + payloadLenghte = (byte & 0X3F) - 3; + pRx = stdRx.data; + parserState = AT10::GIMBAL_SERIAL_STATE_STD_CMD; + break; + + case AT10::GIMBAL_SERIAL_STATE_STD_CMD: + stdRx.cmd = byte; + parserState = AT10::GIMBAL_SERIAL_STATE_STD_DATE; + break; + + case AT10::GIMBAL_SERIAL_STATE_STD_DATE: + *pRx = byte; + pRx++; + payloadLenghte--; + if (payloadLenghte == 0) + { + parserState = AT10::GIMBAL_SERIAL_STATE_STD_CHECK; + } + break; + + case AT10::GIMBAL_SERIAL_STATE_STD_CHECK: + stdRx.checkXOR = byte; + if (AT10::checkXOR((uint8_t *)&stdRx.len, (stdRx.len & 0X3F)) == byte) + { + state = true; + stdRxQueue->inCell(&stdRx); + } + else + { + memset(&stdRx, 0, sizeof(AT10::GIMBAL_STD_FRAME_T)); + } + parserState = AT10::GIMBAL_SERIAL_STATE_IDLE; + payloadLenghte = 0; + pRx = nullptr; + + break; + + // EXT msg + case AT10::GIMBAL_SERIAL_STATE_EXT_HEAD1: + if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0X0000FF00) >> 8)) + { + parserState = AT10::GIMBAL_SERIAL_STATE_EXT_HEAD2; + } + else + { + parserState = AT10::GIMBAL_SERIAL_STATE_IDLE; + } + break; + + case AT10::GIMBAL_SERIAL_STATE_EXT_HEAD2: + if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0X00FF0000) >> 16)) + { + parserState = AT10::GIMBAL_SERIAL_STATE_EXT_HEAD3; + } + else + { + parserState = AT10::GIMBAL_SERIAL_STATE_IDLE; + } + break; + + case AT10::GIMBAL_SERIAL_STATE_EXT_HEAD3: + if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0XFF000000) >> 24)) + { + parserState = AT10::GIMBAL_SERIAL_STATE_EXT_DATE; + payloadLenghte = sizeof(AT10::GIMBAL_RCV_POS_MSG_T); + pRx = extendRx.data; + extendRx.head = AT10::GIMBAL_CMD_RCV_STATE; + } + else + { + parserState = AT10::GIMBAL_SERIAL_STATE_IDLE; + } + break; + + case AT10::GIMBAL_SERIAL_STATE_EXT_DATE: + *pRx = byte; + payloadLenghte--; + pRx++; + if (payloadLenghte == 0) + { + parserState = AT10::GIMBAL_SERIAL_STATE_EXT_CHECK; + } + break; + + case AT10::GIMBAL_SERIAL_STATE_EXT_CHECK: + suncheck = AT10::CheckSum(extendRx.data, sizeof(AT10::GIMBAL_RCV_POS_MSG_T)); + if (byte == suncheck) + { + state = true; + rxQueue->inCell(&extendRx); + } + else + { + memset(&extendRx, 0, sizeof(AT10::GIMBAL_EXTEND_FRAME_T)); + } + parserState = AT10::GIMBAL_SERIAL_STATE_IDLE; + payloadLenghte = 0; + pRx = nullptr; + break; + + default: + parserState = AT10::GIMBAL_SERIAL_STATE_IDLE; + memset(&extendRx, 0, sizeof(AT10::GIMBAL_EXTEND_FRAME_T)); + memset(&stdRx, 0, sizeof(AT10::GIMBAL_STD_FRAME_T)); + payloadLenghte = 0; + pRx = nullptr; + break; + } + + return state; +} + +void AT10GimbalDriver::sendHeart(void) +{ + // uint8_t tempBuffer[72]; + uint8_t temp = 0X00; + while (1) + { + // if (!IO->isBusy() && IO->isOpen()) + // { + // bool state = false; + + // state = txQueue->outCell(&tempBuffer); + // if (state) + // { + // IO->outPutBytes((uint8_t *)&tempBuffer, + // reinterpret_cast(tempBuffer)->len + AT10_EXT_PAYLOAD_OFFSET + sizeof(uint8_t)); + // } + // } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + pack(AT10::GIMBAL_CMD_STD_HEART, &temp, sizeof(temp)); + } +} + +void AT10GimbalDriver::sendStd(void) +{ + uint8_t tempBuffer[72]; + while (1) + { + if (!IO->isBusy() && IO->isOpen()) + { + bool state = false; + + state = stdTxQueue->outCell(&tempBuffer); + if (state) + { + IO->outPutBytes((uint8_t *)&tempBuffer + 1, + reinterpret_cast(tempBuffer)->len + 3); + } + } + } +} + +void AT10GimbalDriver::stackStart(void) +{ + if (!this->IO->isOpen()) + { + this->IO->open(); + } + std::thread sendHeartLoop(&AT10GimbalDriver::sendHeart, this); + std::thread sendStdLoop(&AT10GimbalDriver::sendStd, this); + sendHeartLoop.detach(); + sendStdLoop.detach(); +} + +void AT10GimbalDriver::parserLoop(void) +{ + uint8_t temp; + + while (1) + { + if (IO->inPutByte(&temp)) + { + parser(temp); + } + } +} + +void AT10GimbalDriver::getStdRxPack(void) +{ + uint8_t tempBuffer[280]; + while (1) + { + if (stdRxQueue->outCell(tempBuffer)) + { + msgCustomCallback(tempBuffer); + convert(tempBuffer); + } + } +} + +void AT10GimbalDriver::getExtRxPack(void) +{ + uint8_t tempBuffer[280]; + while (1) + { + if (rxQueue->outCell(tempBuffer)) + { + msgCustomCallback(tempBuffer); + convert(tempBuffer); + } + } +} + +void AT10GimbalDriver::parserStart(amovGimbal::pStateInvoke callback) +{ + this->updateGimbalStateCallback = callback; + + std::thread parser(&AT10GimbalDriver::parserLoop, this); + std::thread getStdRxPackLoop(&AT10GimbalDriver::getStdRxPack, this); + std::thread getExtRxPackLooP(&AT10GimbalDriver::getExtRxPack, this); + + parser.detach(); + getStdRxPackLoop.detach(); + getExtRxPackLooP.detach(); +} + +uint32_t AT10GimbalDriver::calPackLen(void *pack) +{ + return 0; +} \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h new file mode 100755 index 0000000..bf84e97 --- /dev/null +++ b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h @@ -0,0 +1,82 @@ +/* + * @Description: Q10f吊舱的驱动文件 + * @Author: L LC @amov + * @Date: 2022-10-28 12:24:21 + * @LastEditors: L LC @amov + * @LastEditTime: 2023-08-25 19:28:55 + * @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_driver.h + */ +#include "../amov_gimbal.h" +#include "../amov_gimbal_private.h" +#include "AT10_gimbal_struct.h" +#include +#include +#include + +#ifndef __AT10_DRIVER_H +#define __AT10_DRIVER_H + +class AT10GimbalDriver : protected amovGimbal::amovGimbalBase +{ +private: + AT10::GIMBAL_SERIAL_STATE_T parserState; + AT10::GIMBAL_EXTEND_FRAME_T extendRx; + + AT10::GIMBAL_STD_FRAME_T stdRx; + fifoRing *stdRxQueue; + fifoRing *stdTxQueue; + + // void send(void); + + void stackStart(void); + void sendHeart(void); + void sendStd(void); + + void parserStart(amovGimbal::pStateInvoke callback); + void parserLoop(void); + void getExtRxPack(void); + void getStdRxPack(void); + + // bool getRxPack(OUT void *pack); + + bool parser(IN uint8_t byte); + void convert(void *buf); + uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize); + uint32_t calPackLen(void *pack); + +public: + // funtions + uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos); + uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed); + uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed); + uint32_t setGimabalHome(void); + + uint32_t setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); + uint32_t setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); + + uint32_t takePic(void); + uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState); + + uint32_t extensionFuntions(void* cmd); + + // builds + static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO) + { + return new AT10GimbalDriver(_IO); + } + + AT10GimbalDriver(amovGimbal::IOStreamBase *_IO); + ~AT10GimbalDriver() + { + if (stdRxQueue != nullptr) + { + delete stdRxQueue; + } + if (stdTxQueue != nullptr) + { + delete stdTxQueue; + } + } +}; + +#endif diff --git a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp new file mode 100755 index 0000000..14b474f --- /dev/null +++ b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp @@ -0,0 +1,208 @@ +/* + * @Description: + * @Author: L LC @amov + * @Date: 2023-03-02 10:00:52 + * @LastEditors: L LC @amov + * @LastEditTime: 2023-09-07 10:56:15 + * @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_funtion.cpp + */ +#include "AT10_gimbal_driver.h" +#include "AT10_gimbal_crc32.h" +#include "string.h" +#include "math.h" +/** + * It sets the gimbal position. + * + * @param pos the position of the gimbal + * + * @return The return value is the number of bytes written to the buffer. + */ +uint32_t AT10GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos) +{ + int16_t yaw, pitch; + AT10::GIMBAL_CMD_A1_MSG_T temp; + yaw = (int16_t)(pos.yaw / (360.0f / 65536.0f)); + printf("\r\n %04X\r\n", yaw); + yaw = amovGimbalTools::conversionBigLittle((uint16_t)yaw); + pitch = (int16_t)(pos.pitch / (360.0f / 65536.0f)); + pitch = amovGimbalTools::conversionBigLittle((uint16_t)pitch); + temp.cmd = 0x0B; + temp.param[0] = yaw; + temp.param[1] = pitch; + temp.param[2] = 0; + temp.param[3] = 0; + return pack(AT10::GIMBAL_CMD_STD_MOTOR, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_A1_MSG_T)); +} + +/** + * It takes a struct of type amovGimbal::AMOV_GIMBAL_POS_T and converts it to a struct of type + * G1::GIMBAL_SET_POS_MSG_T + * + * @param speed the speed of the gimbal + * + * @return The return value is the number of bytes written to the buffer. + */ +uint32_t AT10GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed) +{ + int16_t speedYaw, speedPitch; + AT10::GIMBAL_CMD_A1_MSG_T temp; + speedYaw = (int16_t)(speed.yaw * 100); + printf("\r\n %04X\r\n", speedYaw); + speedYaw = amovGimbalTools::conversionBigLittle((uint16_t)speedYaw); + speedPitch = (int16_t)(speed.pitch * 100); + speedPitch = amovGimbalTools::conversionBigLittle((uint16_t)speedPitch); + temp.cmd = 0x01; + temp.param[0] = speedYaw; + temp.param[1] = speedPitch; + temp.param[2] = 0; + temp.param[3] = 0; + return pack(AT10::GIMBAL_CMD_STD_MOTOR, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_A1_MSG_T)); +} + +/** + * This function sets the gimbal's follow speed + * + * @param followSpeed the speed of the gimbal + * + * @return The return value is the number of bytes written to the buffer. + */ +uint32_t AT10GimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed) +{ + state.maxFollow.pitch = fabs(followSpeed.pitch * 100); + state.maxFollow.yaw = fabs(followSpeed.yaw * 100); + state.maxFollow.roll = fabs(followSpeed.roll * 100); + return 0; +} + +/** + * This function sets the gimbal to its home position + * + * @return The return value is the number of bytes written to the buffer. + */ +uint32_t AT10GimbalDriver::setGimabalHome(void) +{ + AT10::GIMBAL_CMD_A1_MSG_T temp; + temp.cmd = 0x04; + temp.param[0] = 0; + temp.param[1] = 0; + temp.param[2] = 0; + temp.param[3] = 0; + return pack(AT10::GIMBAL_CMD_STD_MOTOR, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_A1_MSG_T)); +} + +/** + * It takes a picture. + * + * @return The return value is the number of bytes written to the serial port. + */ +uint32_t AT10GimbalDriver::takePic(void) +{ + uint16_t temp = 0x13 << 3; + uint16_t data = amovGimbalTools::conversionBigLittle(temp); + + return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t)); +} + +/** + * The function sets the video state of the gimbal + * + * @param newState The new state of the video. + * + * @return The return value is the number of bytes written to the serial port. + */ +uint32_t AT10GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState) +{ + uint16_t temp; + if (newState == amovGimbal::AMOV_GIMBAL_VIDEO_T::AMOV_GIMBAL_VIDEO_TAKE) + { + temp = 0x14 << 3; + } + else + { + temp = 0x15 << 3; + } + + uint16_t data = amovGimbalTools::conversionBigLittle(temp); + + return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t)); +} + +uint32_t AT10GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate) +{ + if (targetRate == 0.0f) + { + uint16_t temp = 0; + switch (zoom) + { + case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN: + temp = 0X08 << 3; + break; + case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT: + temp = 0X09 << 3; + break; + case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP: + temp = 0X01 << 3; + break; + default: + break; + } + uint16_t data = amovGimbalTools::conversionBigLittle(temp); + + return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t)); + } + else + { + AT10::GIMBAL_CMD_C2_MSG_T temp; + temp.cmd = 0x53; + temp.param = targetRate * 10; + temp.param = amovGimbalTools::conversionBigLittle(temp.param); + return pack(AT10::GIMBAL_CMD_STD_CAMERA2, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_C2_MSG_T)); + } +} + +uint32_t AT10GimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate) +{ + uint16_t temp = 0; + switch (zoom) + { + case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN: + temp = 0X0B << 3; + break; + case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT: + temp = 0X0A << 3; + break; + case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP: + temp = 0X01 << 3; + break; + default: + break; + } + uint16_t data = amovGimbalTools::conversionBigLittle(temp); + + return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t)); +} + +// +/** + * The function `extensionFuntions` in the `AT10GimbalDriver` class takes a command as input, casts it + * to a specific type, and then packs the command and its parameters into a byte array. + * + * @param cmd The "cmd" parameter is a void pointer, which means it can point to any type of data. In + * this case, it is being cast to a pointer of type AT10::AT10_EXT_CMD_T using the reinterpret_cast + * operator. + * + * @return the result of the `pack` function, which is of type `uint32_t`. + */ +uint32_t AT10GimbalDriver::extensionFuntions(void *cmd) +{ + AT10::AT10_EXT_CMD_T *tempCMD; + tempCMD = reinterpret_cast(cmd); + + return pack(tempCMD->cmd, (uint8_t *)tempCMD->param, tempCMD->paramLen); +} + +// AT10_EXT_CMD_T infraredOpen ; +// infraredOpen.cmd = AT10::GIMBAL_CMD_STD_CAMERA; +// infraredOpen.param[0] = 0X02; +// infraredOpen.param[1] = 0; +// infraredOpen.paramLen = 2; \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h new file mode 100755 index 0000000..54f0dee --- /dev/null +++ b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h @@ -0,0 +1,174 @@ +/* + * @Description: + * @Author: L LC @amov + * @Date: 2022-10-27 18:10:07 + * @LastEditors: L LC @amov + * @LastEditTime: 2023-08-25 19:32:59 + * @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_struct.h + */ +#ifndef AT10_GIMBAL_STRUCT_H +#define AT10_GIMBAL_STRUCT_H + +#include +namespace AT10 +{ +#define AT10_MAX_GIMBAL_PAYLOAD 64 +#define AT10_EXT_PAYLOAD_OFFSET 4 +#define AT10_STD_PAYLOAD_OFFSET 6 +#define AT10_EXT_SCALE_FACTOR_ANGLE 0.02197f +#define AT10_EXT_SCALE_FACTOR_SPEED 0.06103f + + typedef enum + { + GIMBAL_CMD_STD_NOP = 0X00, + GIMBAL_CMD_STD_HEART = 0X10, + GIMBAL_CMD_STD_RCV_STATE = 0X40, + GIMBAL_CMD_STD_MOTOR = 0X1A, + GIMBAL_CMD_STD_CAMERA = 0X1C, + GIMBAL_CMD_STD_CAMERA2 = 0X2C, + GIMBAL_CMD_STD_MOTOR2 = 0X32, + GIMBAL_CMD_STD = 0XDCAA5500, + GIMBAL_CMD_RCV_STATE = 0X721A583E, + GIMBAL_CMD_SET_FEEDBACK_L = 0X143055AA, + GIMBAL_CMD_SET_FEEDBACK_H = 0X003155AA, + GIMBAL_CMD_OPEN_FEEDBACK = 0X3E003E3E, + GIMBAL_CMD_CLOSE_FEEDBACK = 0X3D003D3E, + } GIMBAL_CMD_T; + + typedef enum + { + GIMBAL_SERIAL_STATE_IDLE, + GIMBAL_SERIAL_STATE_EXT_HEAD1, + GIMBAL_SERIAL_STATE_EXT_HEAD2, + GIMBAL_SERIAL_STATE_EXT_HEAD3, + GIMBAL_SERIAL_STATE_EXT_DATE, + GIMBAL_SERIAL_STATE_EXT_CHECK, + GIMBAL_SERIAL_STATE_STD_HAED1, + GIMBAL_SERIAL_STATE_STD_HAED2, + GIMBAL_SERIAL_STATE_STD_LEN, + GIMBAL_SERIAL_STATE_STD_CMD, + GIMBAL_SERIAL_STATE_STD_DATE, + GIMBAL_SERIAL_STATE_STD_CHECK, + } GIMBAL_SERIAL_STATE_T; + +#pragma pack(1) + typedef struct + { + uint8_t cmd; + uint8_t param[AT10_MAX_GIMBAL_PAYLOAD]; + uint8_t paramLen; + } AT10_EXT_CMD_T; + + typedef struct + { + uint32_t head; + uint8_t len; + uint8_t cmd; + uint8_t data[AT10_MAX_GIMBAL_PAYLOAD]; + uint8_t checkXOR; + } GIMBAL_STD_FRAME_T; + + typedef struct + { + uint32_t head; + uint8_t data[AT10_MAX_GIMBAL_PAYLOAD]; + uint8_t checkSum; + uint8_t len; + } GIMBAL_EXTEND_FRAME_T; + + typedef struct + { + uint16_t timeStamp; + int16_t rollIMUAngle; + int16_t pitchIMUAngle; + int16_t yawIMUAngle; + int16_t rollTAGAngle; + int16_t pitchTAGAngle; + int16_t yawTAGAngle; + int16_t rollTAGSpeed; + int16_t pitchTAGSpeed; + int16_t yawTAGSpeed; + int16_t rollStatorRotorAngle; + int16_t pitchStatorRotorAngle; + int16_t yawStatorRotorAngle; + } GIMBAL_RCV_POS_MSG_T; + + typedef struct + { + uint8_t hight; + uint8_t reserve; + uint32_t lat; + uint32_t log; + int16_t alt; + uint32_t latTar; + uint32_t logTar; + int16_t altTar; + } GIMBAL_RCV_GPS_STATE_MSG_T; + + typedef struct + { + int16_t roll; + int16_t yaw; + int16_t pitch; + } GIMBAL_RCV_MOTOR_STATE_MSG_T; + + typedef struct + { + uint8_t mode; + uint8_t reserve; + uint16_t camera; + uint16_t distance; + uint16_t fovY; + uint16_t fovX; + uint16_t rate; + } GIMBAL_RCV_CAMERA_STATE_MSG_T; + + typedef struct + { + GIMBAL_RCV_GPS_STATE_MSG_T T1; + uint8_t F1; + GIMBAL_RCV_MOTOR_STATE_MSG_T B1; + GIMBAL_RCV_CAMERA_STATE_MSG_T D1; + } GIMBAL_RCV_STD_STATE_MSG_T; + + typedef struct + { + uint16_t param; + } GIMBAL_CMD_C1_MSG_T; + + typedef struct + { + uint8_t cmd; + uint16_t param; + } GIMBAL_CMD_C2_MSG_T; + + typedef struct + { + uint8_t cmd; + uint16_t param[4]; + } GIMBAL_CMD_A1_MSG_T; + + typedef struct + { + uint8_t cmd; + uint8_t reserve; + uint32_t param[3]; + } GIMBAL_CMD_S1_MSG_T; + + typedef struct + { + uint8_t param[3]; + } GIMBAL_CMD_E1_MSG_T; + + typedef struct + { + GIMBAL_CMD_A1_MSG_T a1; + GIMBAL_CMD_C1_MSG_T c1; + GIMBAL_CMD_E1_MSG_T e1; + GIMBAL_CMD_S1_MSG_T s1; + } GIMBAL_CMD_A1C1E1S1_MSG_T; + +#pragma pack() + +} +#endif diff --git a/gimbal_ctrl/driver/src/CMakeLists.txt b/gimbal_ctrl/driver/src/CMakeLists.txt new file mode 100755 index 0000000..5421bce --- /dev/null +++ b/gimbal_ctrl/driver/src/CMakeLists.txt @@ -0,0 +1,36 @@ +add_library(AMOV_Gimbal ${LIB_FLAG}) + +SET(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g2 -ggdb -fPIC") +SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall -fPIC") + +add_definitions( + -DMAX_QUEUE_SIZE=100 +) + +add_subdirectory(FIFO) +########## add types of gimbal ############## +add_subdirectory(G1) +add_subdirectory(G2) +add_subdirectory(Q10f) +add_subdirectory(AT10) + +file(GLOB LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp) + +target_sources(AMOV_Gimbal + PRIVATE + ${LIB_FILES} +) + +target_link_libraries(AMOV_Gimbal + PRIVATE + Gimabl_G1 + Gimabl_G2 + Gimabl_Q10f + Gimabl_AT10 +) + +target_include_directories(AMOV_Gimbal + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + diff --git a/gimbal_ctrl/driver/src/FIFO/CMakeLists.txt b/gimbal_ctrl/driver/src/FIFO/CMakeLists.txt new file mode 100755 index 0000000..2357221 --- /dev/null +++ b/gimbal_ctrl/driver/src/FIFO/CMakeLists.txt @@ -0,0 +1,13 @@ +add_library(FIFO) + +file(GLOB LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp) + +target_sources(FIFO + PRIVATE + ${LIB_FILES} +) + +target_include_directories(FIFO + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cc b/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cc deleted file mode 100755 index ba507a3..0000000 --- a/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cc +++ /dev/null @@ -1,212 +0,0 @@ -/* - * @Description : - * @Author : Aiyangsky - * @Date : 2022-08-26 21:42:10 - * @LastEditors : Aiyangsky - * @LastEditTime : 2022-08-27 03:43:49 - * @FilePath : \mavlink\src\route\Ring_Fifo.c - */ - -#include - -#include "Ring_Fifo.h" - -/** - * @description: - * @param {RING_FIFO_CB_T} *fifo fifo struct pointer - * @param {unsigned short} cell_size sizeof(cell) - * @param {unsigned char} *buffer fifo buffer address - * @param {unsigned int} buffer_lenght sizeof(buffer) - * @return {*} - * @note : - */ -void Ring_Fifo_init(RING_FIFO_CB_T *fifo, unsigned short cell_size, - unsigned char *buffer, unsigned int buffer_lenght) -{ - fifo->cell_size = cell_size; - - fifo->start = buffer; - // Remainder is taken to avoid splicing in the output so as to improve the efficiency - fifo->end = buffer + buffer_lenght - (buffer_lenght % cell_size); - fifo->in = buffer; - fifo->out = buffer; - fifo->curr_number = 0; - fifo->max_number = buffer_lenght / cell_size; -} - -/** - * @description: add a cell to fifo - * @param {RING_FIFO_CB_T} *fifo fifo struct pointer - * @param {void} *data cell data [in] - * @return {*} Success or fail - * @note : failed if without space - */ -bool Ring_Fifo_in_cell(RING_FIFO_CB_T *fifo, void *data) -{ - unsigned char *next; - unsigned char *ptemp = fifo->in; - bool ret = false; - - LOCK(); - - if (fifo->curr_number < fifo->max_number) - { - next = fifo->in + fifo->cell_size; - if (next >= fifo->end) - { - next = fifo->start; - } - fifo->in = next; - fifo->curr_number++; - memcpy(ptemp, data, fifo->cell_size); - ret = true; - } - - UNLOCK(); - - return ret; -} - -/** - * @description: add a series of cells to fifo - * @param {RING_FIFO_CB_T} *fifo - * @param {void} *data cells data [in] - * @param {unsigned short} number expect add number of cells - * @return {*} number of successful add - * @note : - */ -unsigned short Ring_Fifo_in_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number) -{ - // Number of remaining storable cells is described to simplify the calculation in the copying process. - unsigned short diff = fifo->max_number - fifo->curr_number; - unsigned short count_temp, count_temp_r; - unsigned char *next; - unsigned char *ptemp = fifo->in; - unsigned short ret; - - LOCK(); - - if (diff > number) - { - ret = number; - } - else if (diff > 0 && diff < number) - { - ret = diff; - } - else - { - ret = 0; - } - - count_temp = fifo->cell_size * ret; - next = fifo->in + count_temp; - // Moving the write pointer and the number of stored cells before - // copying data reduces the likelihood of multithreaded write conflicts. - fifo->curr_number += ret; - - if (next < fifo->end) - { - fifo->in = next; - memcpy(ptemp, data, count_temp); - } - else - { - count_temp_r = fifo->end - fifo->in; - next = fifo->start + count_temp - count_temp_r; - fifo->in = next; - memcpy(ptemp, data, count_temp_r); - memcpy(fifo->start, ((unsigned char *)data) + count_temp_r, count_temp - count_temp_r); - } - - UNLOCK(); - - return ret; -} - -/** - * @description: output a cell - * @param {RING_FIFO_CB_T} *fifo - * @param {void} *data cell data [out] - * @return {*} Success or fail - * @note : fail if without cell - */ -bool Ring_Fifo_out_cell(RING_FIFO_CB_T *fifo, void *data) -{ - unsigned char *next; - unsigned char *ptemp = fifo->out; - bool ret = false; - - LOCK(); - - if (fifo->curr_number > 0) - { - next = fifo->out + fifo->cell_size; - if (next >= fifo->end) - { - next = fifo->start; - } - fifo->out = next; - fifo->curr_number--; - memcpy(data, ptemp, fifo->cell_size); - ret = true; - } - - UNLOCK(); - - return ret; -} - -/** - * @description: output a series of cells in fifo - * @param {RING_FIFO_CB_T} *fifo - * @param {void} *data cells data [out] - * @param {unsigned short} number expect out number of cells - * @return {*} number of successful output - * @note : - */ -unsigned short Ring_Fifo_out_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number) -{ - unsigned char *next; - unsigned char *ptemp = fifo->out; - unsigned short count_temp, count_temp_r; - unsigned short ret; - - LOCK(); - - if (fifo->curr_number > number) - { - ret = number; - } - else if (fifo->curr_number < number && fifo->curr_number > 0) - { - ret = fifo->curr_number; - } - else - { - ret = 0; - } - - count_temp = fifo->cell_size * ret; - next = fifo->out + count_temp; - - fifo->curr_number -= ret; - - if (next < fifo->end) - { - fifo->out = next; - memcpy(data, ptemp, count_temp); - } - else - { - count_temp_r = fifo->end - fifo->in; - next = fifo->start + count_temp - count_temp_r; - fifo->out = next; - memcpy(data, ptemp, count_temp_r); - memcpy(((unsigned char *)data) + count_temp_r, fifo->start, count_temp - count_temp_r); - } - - UNLOCK(); - - return ret; -} diff --git a/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cpp b/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cpp new file mode 100755 index 0000000..87585a0 --- /dev/null +++ b/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cpp @@ -0,0 +1,228 @@ +/* + * @Description : + * @Author : Aiyangsky + * @Date : 2022-08-26 21:42:10 + * @LastEditors: L LC @amov + * @LastEditTime: 2023-07-21 16:10:16 + * @FilePath: /gimbal-sdk-multi-platform/src/FIFO/Ring_Fifo.cpp + */ + +#include + +#include "Ring_Fifo.h" + +/** + * The `fifoRing` constructor initializes the `cellSize`, `maxNumber`, `currNumber`, `start`, `in`, + * `out`, and `end` variables, and allocates memory for the `start` pointer. + * + * @param _cellSize The `_cellSize` parameter represents the size of each cell in the FIFO ring buffer. + * It determines the amount of memory allocated for each element in the buffer. + * @param _cellNum The parameter `_cellNum` represents the number of cells in the FIFO ring. + */ +fifoRing::fifoRing(unsigned short _cellSize, unsigned int _cellNum) +{ + cellSize = _cellSize; + maxNumber = _cellNum; + currNumber = 0; + + start = nullptr; + start = (uint8_t *)malloc(_cellNum * _cellSize); + if (start == nullptr) + { + std::cout << "fifo malloc failed! size :" << (_cellNum * _cellSize) << std::endl; + exit(1); + } + + memset(start, 0, _cellNum * _cellSize); + + in = start; + out = start; + end = start + _cellNum * _cellSize; +} + +/** + * The `inCell` function adds data to a FIFO ring buffer and returns true if successful. + * + * @param data A pointer to the data that needs to be stored in the FIFO ring. + * + * @return a boolean value. + */ +bool fifoRing::inCell(void *data) +{ + std::lock_guard locker(fifoMutex); + unsigned char *next; + unsigned char *ptemp = in; + bool ret = false; + + if (currNumber < maxNumber) + { + next = in + cellSize; + if (next >= end) + { + next = start; + } + in = next; + currNumber++; + memcpy(ptemp, data, cellSize); + ret = true; + notEmpty.notify_all(); + } + + return ret; +} + +/** + * The `inCells` function is used to store data in a FIFO ring buffer, returning the number of cells + * successfully stored. + * + * @param data A pointer to the data that needs to be stored in the FIFO ring. + * @param number The parameter "number" represents the number of cells that the function should attempt + * to store in the FIFO ring. + * + * @return the number of cells that were successfully stored in the FIFO ring buffer. + */ +unsigned short fifoRing::inCells(void *data, unsigned short number) +{ + std::lock_guard locker(fifoMutex); + // Number of remaining storable cells is described to simplify the calculation in the copying process. + unsigned short diff = maxNumber - currNumber; + unsigned short count_temp, count_temp_r; + unsigned char *next; + unsigned char *ptemp = in; + unsigned short ret; + + if (diff > number) + { + ret = number; + } + else if (diff > 0 && diff < number) + { + ret = diff; + } + else + { + ret = 0; + } + + count_temp = cellSize * ret; + next = in + count_temp; + // Moving the write pointer and the number of stored cells before + // copying data reduces the likelihood of multithreaded write conflicts. + currNumber += ret; + + if (next < end) + { + in = next; + memcpy(ptemp, data, count_temp); + } + else + { + count_temp_r = end - in; + next = start + count_temp - count_temp_r; + in = next; + memcpy(ptemp, data, count_temp_r); + memcpy(start, ((unsigned char *)data) + count_temp_r, count_temp - count_temp_r); + } + if (ret > 0) + { + notEmpty.notify_all(); + } + + return ret; +} + +/** + * The `outCell` function removes a cell from the FIFO ring buffer and copies its data to the provided + * memory location. + * + * @param data A pointer to the memory location where the data from the cell will be copied to. + * + * @return a boolean value. If a cell is successfully taken from the FIFO ring and the data is copied + * into the provided pointer, the function returns true. Otherwise, if the FIFO ring is empty and no + * cell can be taken, the function waits until a cell becomes available and then returns false. + */ +bool fifoRing::outCell(void *data) +{ + std::lock_guard locker(fifoMutex); + unsigned char *next; + unsigned char *ptemp = out; + bool ret = false; + + if (currNumber > 0) + { + next = out + cellSize; + if (next >= end) + { + next = start; + } + out = next; + currNumber--; + memcpy(data, ptemp, cellSize); + ret = true; + } + else + { + notEmpty.wait(fifoMutex); + } + + return ret; +} + +/** + * The `outCells` function retrieves a specified number of cells from a FIFO ring buffer and copies the + * data into a provided buffer. + * + * @param data A pointer to the memory location where the extracted data will be stored. + * @param number The parameter "number" represents the number of cells that should be read from the + * FIFO ring. + * + * @return the number of cells that were successfully read from the FIFO ring buffer. + */ +unsigned short fifoRing::outCells(void *data, unsigned short number) +{ + std::lock_guard locker(fifoMutex); + + unsigned char *next; + unsigned char *ptemp = out; + unsigned short count_temp, count_temp_r; + unsigned short ret; + + if (currNumber > number) + { + ret = number; + } + else if (currNumber < number && currNumber > 0) + { + ret = currNumber; + } + else + { + ret = 0; + } + + count_temp = cellSize * ret; + next = out + count_temp; + + currNumber -= ret; + + if (next < end) + { + out = next; + memcpy(data, ptemp, count_temp); + } + else + { + count_temp_r = end - in; + next = start + count_temp - count_temp_r; + out = next; + memcpy(data, ptemp, count_temp_r); + memcpy(((unsigned char *)data) + count_temp_r, start, count_temp - count_temp_r); + } + + if (ret == 0) + { + notEmpty.wait(fifoMutex); + } + + return ret; +} diff --git a/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.h b/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.h index f50087c..96c3e63 100755 --- a/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.h +++ b/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.h @@ -3,45 +3,47 @@ * @Author : Aiyangsky * @Date : 2022-08-26 21:42:02 * @LastEditors: L LC @amov - * @LastEditTime: 2023-03-03 16:12:37 - * @FilePath: \host\gimbal-sdk-multi-platform\src\FIFO\Ring_Fifo.h + * @LastEditTime: 2023-08-16 21:22:46 + * @FilePath: /gimbal-sdk-multi-platform/src/FIFO/Ring_Fifo.h */ #ifndef RING_FIFO_H #define RING_FIFO_H -#include "stdbool.h" +#include +#include +#include +#include -#ifdef __cplusplus -extern "C" +class fifoRing { -#endif +private: + unsigned char *start; + unsigned char *in; + unsigned char *out; + unsigned char *end; -#define LOCK() -#define UNLOCK() + unsigned short currNumber; + unsigned short maxNumber; + unsigned short cellSize; - typedef struct + std::mutex fifoMutex; + std::condition_variable_any notEmpty; + +public: + fifoRing(unsigned short _cellSize, unsigned int _cellNum); + ~fifoRing() { - unsigned char *start; - unsigned char *in; - unsigned char *out; - unsigned char *end; + if (start != nullptr) + { + free(start); + } + } - unsigned short curr_number; - unsigned short max_number; - unsigned short cell_size; - } RING_FIFO_CB_T; - - void Ring_Fifo_init(RING_FIFO_CB_T *fifo, unsigned short cell_size, - unsigned char *buffer, unsigned int buffer_lenght); - bool Ring_Fifo_in_cell(RING_FIFO_CB_T *fifo, void *data); - unsigned short Ring_Fifo_in_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number); - bool Ring_Fifo_out_cell(RING_FIFO_CB_T *fifo, void *data); - unsigned short Ring_Fifo_out_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number); - - -#ifdef __cplusplus -} -#endif + bool inCell(void *data); + unsigned short inCells(void *data, unsigned short number); + bool outCell(void *data); + unsigned short outCells(void *data, unsigned short number); +}; #endif \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp b/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp index 5748722..7484938 100755 --- a/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp +++ b/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:06 * @LastEditors: L LC @amov - * @LastEditTime: 2023-04-18 10:12:46 + * @LastEditTime: 2023-09-07 11:01:25 * @FilePath: /gimbal-sdk-multi-platform/src/G1/g1_gimbal_driver.cpp */ #include "g1_gimbal_driver.h" @@ -16,27 +16,10 @@ * * @param _IO The IOStreamBase object that will be used to communicate with the gimbal. */ -g1GimbalDriver::g1GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(_IO) +g1GimbalDriver::g1GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::amovGimbalBase(_IO) { - memset(&rxQueue, 0, sizeof(RING_FIFO_CB_T)); - memset(&txQueue, 0, sizeof(RING_FIFO_CB_T)); - - rxBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T)); - if (rxBuffer == NULL) - { - std::cout << "Receive buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl; - exit(1); - } - txBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T)); - if (txBuffer == NULL) - { - free(rxBuffer); - std::cout << "Send buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl; - exit(1); - } - - Ring_Fifo_init(&rxQueue, sizeof(G1::GIMBAL_FRAME_T), rxBuffer, MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T)); - Ring_Fifo_init(&txQueue, sizeof(G1::GIMBAL_FRAME_T), txBuffer, MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T)); + rxQueue = new fifoRing(sizeof(G1::GIMBAL_FRAME_T), MAX_QUEUE_SIZE); + txQueue = new fifoRing(sizeof(G1::GIMBAL_FRAME_T), MAX_QUEUE_SIZE); parserState = G1::GIMBAL_SERIAL_STATE_IDLE; } @@ -66,32 +49,14 @@ uint32_t g1GimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloa txTemp.crc.u32 = G1::CRC32Software(txTemp.payload, payloadSize); memcpy(txTemp.payload + payloadSize, txTemp.crc.u8, sizeof(uint32_t)); - txMutex.lock(); - if (Ring_Fifo_in_cell(&txQueue, &txTemp)) + if (txQueue->inCell(&txTemp)) { ret = txTemp.lenght + G1_PAYLOAD_OFFSET + sizeof(uint32_t); } - txMutex.unlock(); return ret; } -/** - * > This function is used to get a packet from the receive queue - * - * @param void This is the type of data that will be stored in the queue. - * - * @return A boolean value. - */ -bool g1GimbalDriver::getRxPack(OUT void *pack) -{ - bool state = false; - rxMutex.lock(); - state = Ring_Fifo_out_cell(&rxQueue, pack); - rxMutex.unlock(); - return state; -} - void g1GimbalDriver::convert(void *buf) { G1::GIMBAL_FRAME_T *temp; @@ -125,32 +90,9 @@ void g1GimbalDriver::convert(void *buf) } } -/** - * The function is called by the main thread to send a command to the gimbal. - * - * The function first checks to see if the serial port is busy and if it is open. If it is not busy and - * it is open, the function locks the txMutex and then checks to see if there is a command in the - * txQueue. If there is a command in the txQueue, the function copies the command to the tx buffer and - * then unlocks the txMutex. The function then sends the command to the gimbal. - * - * The txQueue is a ring buffer that holds commands that are waiting to be sent to the gimbal. The - * txQueue is a ring buffer because the gimbal can only process one command at a time. If the gimbal is - * busy processing a command, the command will be placed in the txQueue and sent to the gimbal when the - * gimbal is ready to receive the command. - */ -void g1GimbalDriver::send(void) +uint32_t g1GimbalDriver::calPackLen(void *pack) { - if (!IO->isBusy() && IO->isOpen()) - { - bool state = false; - txMutex.lock(); - state = Ring_Fifo_out_cell(&txQueue, &tx); - txMutex.unlock(); - if (state) - { - IO->outPutBytes((uint8_t *)&tx, tx.lenght + G1_PAYLOAD_OFFSET + sizeof(uint32_t)); - } - } + return ((G1::GIMBAL_FRAME_T *)pack)->lenght + G1_PAYLOAD_OFFSET + sizeof(uint32_t); } /** @@ -225,9 +167,7 @@ bool g1GimbalDriver::parser(IN uint8_t byte) if (*((uint32_t *)(pRx - sizeof(uint32_t))) == G1::CRC32Software(rx.payload, rx.lenght)) { state = true; - rxMutex.lock(); - Ring_Fifo_in_cell(&rxQueue, &rx); - rxMutex.unlock(); + rxQueue->inCell(&rx); } else { diff --git a/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h b/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h index 19ed884..c293fd2 100755 --- a/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h +++ b/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h @@ -3,10 +3,11 @@ * @Author: L LC @amov * @Date: 2022-10-28 12:24:21 * @LastEditors: L LC @amov - * @LastEditTime: 2023-03-13 12:29:17 - * @FilePath: \gimbal-sdk-multi-platform\src\G1\g1_gimbal_driver.h + * @LastEditTime: 2023-09-07 02:31:19 + * @FilePath: /gimbal-sdk-multi-platform/src/G1/g1_gimbal_driver.h */ #include "../amov_gimbal.h" +#include "../amov_gimbal_private.h" #include "g1_gimbal_struct.h" #include #include @@ -15,31 +16,16 @@ #ifndef __G1_DRIVER_H #define __G1_DRIVER_H -extern "C" -{ -#include "Ring_Fifo.h" -} - -class g1GimbalDriver : protected amovGimbal::IamovGimbalBase +class g1GimbalDriver : protected amovGimbal::amovGimbalBase { private: G1::GIMBAL_CMD_PARSER_STATE_T parserState; G1::GIMBAL_FRAME_T rx; - G1::GIMBAL_FRAME_T tx; - std::mutex rxMutex; - uint8_t *rxBuffer; - RING_FIFO_CB_T rxQueue; - std::mutex txMutex; - uint8_t *txBuffer; - RING_FIFO_CB_T txQueue; - - bool parser(IN uint8_t byte); - void send(void); - - void convert(void *buf); uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize); - bool getRxPack(OUT void *pack); + bool parser(IN uint8_t byte); + void convert(void *buf); + uint32_t calPackLen(void *pack); public: // funtions @@ -51,18 +37,24 @@ public: uint32_t takePic(void); uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState); + uint32_t attitudeCorrection(const amovGimbal::AMOV_GIMBAL_QUATERNION_T &quaterion, + const amovGimbal::AMOV_GIMBAL_VELOCITY_T &speed, + const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, + void *extenData); + + uint32_t attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos, + const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, + const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, + void *extenData); + uint32_t extensionFuntions(void *cmd); + // builds - static amovGimbal::IamovGimbalBase *creat(amovGimbal::IOStreamBase *_IO) + static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO) { return new g1GimbalDriver(_IO); } g1GimbalDriver(amovGimbal::IOStreamBase *_IO); - ~g1GimbalDriver() - { - free(rxBuffer); - free(txBuffer); - } }; #endif diff --git a/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp b/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp index 270fbb4..590dbcd 100755 --- a/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp +++ b/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp @@ -3,12 +3,13 @@ * @Author: L LC @amov * @Date: 2023-03-02 10:00:52 * @LastEditors: L LC @amov - * @LastEditTime: 2023-03-17 18:29:33 - * @FilePath: \gimbal-sdk-multi-platform\src\G1\g1_gimbal_funtion.cpp + * @LastEditTime: 2023-09-07 10:50:30 + * @FilePath: /gimbal-sdk-multi-platform/src/G1/g1_gimbal_funtion.cpp */ #include "g1_gimbal_driver.h" #include "g1_gimbal_crc32.h" #include "string.h" +#include /** * It sets the gimbal position. @@ -83,7 +84,7 @@ uint32_t g1GimbalDriver::setGimabalHome(void) /** * It takes a picture. - * + * * @return The return value is the number of bytes written to the serial port. */ uint32_t g1GimbalDriver::takePic(void) @@ -94,9 +95,9 @@ uint32_t g1GimbalDriver::takePic(void) /** * The function sets the video state of the gimbal - * + * * @param newState The new state of the video. - * + * * @return The return value is the number of bytes written to the serial port. */ uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState) @@ -104,7 +105,7 @@ uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState uint8_t temp = G1::GIMBAL_CMD_CAMERA_REC; mState.lock(); - if(state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE) + if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE) { state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF; } @@ -116,3 +117,107 @@ uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState return pack(G1::GIMBAL_CMD_CAMERA, &temp, sizeof(uint8_t)); } + +/** + * The function `attitudeCorrection` takes in quaternion, velocity, acceleration, and external data, + * and returns a packed message. + * + * @param quaterion The "quaterion" parameter is a structure of type "AMOV_GIMBAL_QUATERNION_T" which + * contains the following fields: + * @param speed The "speed" parameter is of type `amovGimbal::AMOV_GIMBAL_VELOCITY_T` and represents + * the velocity of the gimbal. It contains three components: `x`, `y`, and `z`, which represent the + * velocity in the respective axes. + * @param acc The "acc" parameter is of type "amovGimbal::AMOV_GIMBAL_VELOCITY_T" and represents the + * acceleration of the gimbal in three dimensions (x, y, z). + * @param extenData The extenData parameter is a void pointer that can be used to pass additional data + * to the attitudeCorrection function. In this case, it is being cast to a float pointer and then + * accessed as an array. The first element of the array is assigned to the temp.yawSetPoint variable, + * and + * + * @return a uint32_t value. + */ +uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_QUATERNION_T &quaterion, + const amovGimbal::AMOV_GIMBAL_VELOCITY_T &speed, + const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, + void *extenData) +{ + G1::GIMBAL_ATT_CORR_MSG_T temp; + temp.q[0] = quaterion.q0; + temp.q[1] = quaterion.q1; + temp.q[2] = quaterion.q2; + temp.q[3] = quaterion.q3; + + temp.acc[0] = acc.x; + temp.acc[1] = acc.y; + temp.acc[2] = acc.z; + + temp.yawSetPoint = ((float *)extenData)[0]; + temp.yawSpeedSetPoint = ((float *)extenData)[1]; + + return pack(G1::GIMBAL_CMD_SET_STATE, reinterpret_cast(&temp), sizeof(G1::GIMBAL_ATT_CORR_MSG_T)); +} + +/** + * The function `attitudeCorrection` calculates the attitude correction for a gimbal based on the given + * position, velocity, and acceleration values. + * + * @param pos The "pos" parameter is of type amovGimbal::AMOV_GIMBAL_POS_T and represents the current + * position of the gimbal. It contains the pitch, roll, and yaw angles of the gimbal. + * @param seppd seppd stands for "Separate Pointing Device" and it represents the velocity of the + * gimbal in terms of pitch, roll, and yaw. It is of type `amovGimbal::AMOV_GIMBAL_VELOCITY_T` which + * likely contains three float values for pitch, + * @param acc The "acc" parameter is of type "amovGimbal::AMOV_GIMBAL_VELOCITY_T" and represents the + * acceleration of the gimbal in three dimensions (x, y, z). + * @param extenData The `extenData` parameter is a void pointer that can be used to pass additional + * data to the `attitudeCorrection` function. In this code snippet, it is assumed that `extenData` is a + * pointer to a float array with two elements. + * + * @return a uint32_t value. + */ +uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos, + const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, + const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, + void *extenData) +{ + G1::GIMBAL_ATT_CORR_MSG_T temp; + + float pitch = pos.pitch * 0.5f; + float roll = pos.roll * 0.5f; + float yaw = pos.yaw * 0.5f; + + temp.q[0] = cosf(pitch) * cosf(roll) * cosf(yaw) + + sinf(pitch) * sinf(roll) * sinf(yaw); + temp.q[1] = cosf(pitch) * sinf(roll) * cosf(yaw) - + sinf(pitch) * cosf(roll) * sinf(yaw); + temp.q[2] = sinf(pitch) * cosf(roll) * cosf(yaw) + + cosf(pitch) * sinf(roll) * sinf(yaw); + temp.q[3] = cosf(pitch) * sinf(roll) * sinf(yaw) - + sinf(pitch) * cosf(roll) * cosf(yaw); + + temp.acc[0] = acc.x; + temp.acc[1] = acc.y; + temp.acc[2] = acc.z; + + temp.yawSetPoint = ((float *)extenData)[0]; + temp.yawSpeedSetPoint = ((float *)extenData)[1]; + + return pack(G1::GIMBAL_CMD_SET_STATE, reinterpret_cast(&temp), sizeof(G1::GIMBAL_ATT_CORR_MSG_T)); +} + +/** + * The function `extensionFuntions` in the `g1GimbalDriver` class takes a void pointer `cmd`, casts it + * to a `G1::GIMBAL_STD_MSG_T` pointer, and returns the result of calling the `pack` function with the + * `cmd`'s `cmd`, `data`, and `len` members as arguments. + * + * @param cmd The "cmd" parameter is a void pointer, which means it can point to any type of data. In + * this case, it is being cast to a G1::GIMBAL_STD_MSG_T pointer using reinterpret_cast. + * + * @return the result of the `pack` function, which is of type `uint32_t`. + */ +uint32_t g1GimbalDriver::extensionFuntions(void *cmd) +{ + G1::GIMBAL_STD_MSG_T *tempCmd; + + tempCmd = reinterpret_cast(cmd); + return pack(tempCmd->cmd, tempCmd->data, tempCmd->len); +} \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h b/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h index c7e99c5..f551a41 100755 --- a/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h +++ b/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h @@ -3,8 +3,8 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:07 * @LastEditors: L LC @amov - * @LastEditTime: 2023-03-17 18:12:57 - * @FilePath: \gimbal-sdk-multi-platform\src\G1\g1_gimbal_struct.h + * @LastEditTime: 2023-09-07 10:45:13 + * @FilePath: /gimbal-sdk-multi-platform/src/G1/g1_gimbal_struct.h */ #ifndef G1_GIMBAL_STRUCT_H #define G1_GIMBAL_STRUCT_H @@ -20,6 +20,7 @@ namespace G1 typedef enum { + GIMBAL_CMD_SET_STATE = 0X01, GIMBAL_CMD_SET_POS = 0X85, GIMBAL_CMD_CAMERA = 0X86, GIMBAL_CMD_RCV_POS = 0X87 @@ -85,6 +86,22 @@ namespace G1 int16_t HALL_yaw; } GIMBAL_RCV_POS_MSG_T; + typedef struct + { + float q[4]; + float acc[3]; + float yawSetPoint; + float yawSpeedSetPoint; + } GIMBAL_ATT_CORR_MSG_T; + + + typedef struct + { + uint8_t cmd; + uint8_t data[256]; + uint8_t len; + }GIMBAL_STD_MSG_T; + #pragma pack() } diff --git a/gimbal_ctrl/driver/src/G2/g2_gimbal_crc.h b/gimbal_ctrl/driver/src/G2/g2_gimbal_crc.h deleted file mode 100755 index fff2e6c..0000000 --- a/gimbal_ctrl/driver/src/G2/g2_gimbal_crc.h +++ /dev/null @@ -1,166 +0,0 @@ - -#ifndef __G2_GIMBAL_CHECK_H -#define __G2_GIMBAL_CHECK_H - -namespace G2 -{ -#include "stdint.h" - - const uint16_t crc16_tab[256] = { - 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, - 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, - 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, - 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, - 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, - 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, - 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, - 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, - 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, - 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, - 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, - 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, - 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, - 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, - 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, - 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, - 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, - 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, - 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, - 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, - 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, - 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, - 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, - 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, - 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, - 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, - 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, - 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, - 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, - 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, - 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, - 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0}; - - /** - * "For each byte in the data, shift the CRC register left by 8 bits, XOR the CRC register with the CRC - * table value for the byte, and then shift the CRC register right by 8 bits." - * - * The CRC table is a 256-byte array of 16-bit values. The index into the table is the byte value. - * The value in the table is the CRC value for that byte. The CRC table is generated by the following - * function: - * - * @param data pointer to the data to be checked - * @param len the length of the data to be checked - * - * @return The CRC value. - * @note 16 bit CRC with polynomial x^16+x^12+x^5+1 - */ - static inline uint16_t checkCrc16(uint8_t *pData, uint32_t len) - { - uint16_t crc = 0XFFFF; - uint32_t idx = 0; - - for (idx = 0; idx < len; idx++) - { - crc = crc16_tab[((crc >> 8) ^ pData[idx]) & 0xFF] ^ (crc << 8); - } - return crc; - } - - const unsigned int Crc32Table[256] = { - 0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, - 0x1A864DB2, 0x1E475005, 0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, - 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD, 0x4C11DB70, 0x48D0C6C7, - 0x4593E01E, 0x4152FDA9, 0x5F15ADAC, 0x5BD4B01B, 0x569796C2, 0x52568B75, - 0x6A1936C8, 0x6ED82B7F, 0x639B0DA6, 0x675A1011, 0x791D4014, 0x7DDC5DA3, - 0x709F7B7A, 0x745E66CD, 0x9823B6E0, 0x9CE2AB57, 0x91A18D8E, 0x95609039, - 0x8B27C03C, 0x8FE6DD8B, 0x82A5FB52, 0x8664E6E5, 0xBE2B5B58, 0xBAEA46EF, - 0xB7A96036, 0xB3687D81, 0xAD2F2D84, 0xA9EE3033, 0xA4AD16EA, 0xA06C0B5D, - 0xD4326D90, 0xD0F37027, 0xDDB056FE, 0xD9714B49, 0xC7361B4C, 0xC3F706FB, - 0xCEB42022, 0xCA753D95, 0xF23A8028, 0xF6FB9D9F, 0xFBB8BB46, 0xFF79A6F1, - 0xE13EF6F4, 0xE5FFEB43, 0xE8BCCD9A, 0xEC7DD02D, 0x34867077, 0x30476DC0, - 0x3D044B19, 0x39C556AE, 0x278206AB, 0x23431B1C, 0x2E003DC5, 0x2AC12072, - 0x128E9DCF, 0x164F8078, 0x1B0CA6A1, 0x1FCDBB16, 0x018AEB13, 0x054BF6A4, - 0x0808D07D, 0x0CC9CDCA, 0x7897AB07, 0x7C56B6B0, 0x71159069, 0x75D48DDE, - 0x6B93DDDB, 0x6F52C06C, 0x6211E6B5, 0x66D0FB02, 0x5E9F46BF, 0x5A5E5B08, - 0x571D7DD1, 0x53DC6066, 0x4D9B3063, 0x495A2DD4, 0x44190B0D, 0x40D816BA, - 0xACA5C697, 0xA864DB20, 0xA527FDF9, 0xA1E6E04E, 0xBFA1B04B, 0xBB60ADFC, - 0xB6238B25, 0xB2E29692, 0x8AAD2B2F, 0x8E6C3698, 0x832F1041, 0x87EE0DF6, - 0x99A95DF3, 0x9D684044, 0x902B669D, 0x94EA7B2A, 0xE0B41DE7, 0xE4750050, - 0xE9362689, 0xEDF73B3E, 0xF3B06B3B, 0xF771768C, 0xFA325055, 0xFEF34DE2, - 0xC6BCF05F, 0xC27DEDE8, 0xCF3ECB31, 0xCBFFD686, 0xD5B88683, 0xD1799B34, - 0xDC3ABDED, 0xD8FBA05A, 0x690CE0EE, 0x6DCDFD59, 0x608EDB80, 0x644FC637, - 0x7A089632, 0x7EC98B85, 0x738AAD5C, 0x774BB0EB, 0x4F040D56, 0x4BC510E1, - 0x46863638, 0x42472B8F, 0x5C007B8A, 0x58C1663D, 0x558240E4, 0x51435D53, - 0x251D3B9E, 0x21DC2629, 0x2C9F00F0, 0x285E1D47, 0x36194D42, 0x32D850F5, - 0x3F9B762C, 0x3B5A6B9B, 0x0315D626, 0x07D4CB91, 0x0A97ED48, 0x0E56F0FF, - 0x1011A0FA, 0x14D0BD4D, 0x19939B94, 0x1D528623, 0xF12F560E, 0xF5EE4BB9, - 0xF8AD6D60, 0xFC6C70D7, 0xE22B20D2, 0xE6EA3D65, 0xEBA91BBC, 0xEF68060B, - 0xD727BBB6, 0xD3E6A601, 0xDEA580D8, 0xDA649D6F, 0xC423CD6A, 0xC0E2D0DD, - 0xCDA1F604, 0xC960EBB3, 0xBD3E8D7E, 0xB9FF90C9, 0xB4BCB610, 0xB07DABA7, - 0xAE3AFBA2, 0xAAFBE615, 0xA7B8C0CC, 0xA379DD7B, 0x9B3660C6, 0x9FF77D71, - 0x92B45BA8, 0x9675461F, 0x8832161A, 0x8CF30BAD, 0x81B02D74, 0x857130C3, - 0x5D8A9099, 0x594B8D2E, 0x5408ABF7, 0x50C9B640, 0x4E8EE645, 0x4A4FFBF2, - 0x470CDD2B, 0x43CDC09C, 0x7B827D21, 0x7F436096, 0x7200464F, 0x76C15BF8, - 0x68860BFD, 0x6C47164A, 0x61043093, 0x65C52D24, 0x119B4BE9, 0x155A565E, - 0x18197087, 0x1CD86D30, 0x029F3D35, 0x065E2082, 0x0B1D065B, 0x0FDC1BEC, - 0x3793A651, 0x3352BBE6, 0x3E119D3F, 0x3AD08088, 0x2497D08D, 0x2056CD3A, - 0x2D15EBE3, 0x29D4F654, 0xC5A92679, 0xC1683BCE, 0xCC2B1D17, 0xC8EA00A0, - 0xD6AD50A5, 0xD26C4D12, 0xDF2F6BCB, 0xDBEE767C, 0xE3A1CBC1, 0xE760D676, - 0xEA23F0AF, 0xEEE2ED18, 0xF0A5BD1D, 0xF464A0AA, 0xF9278673, 0xFDE69BC4, - 0x89B8FD09, 0x8D79E0BE, 0x803AC667, 0x84FBDBD0, 0x9ABC8BD5, 0x9E7D9662, - 0x933EB0BB, 0x97FFAD0C, 0xAFB010B1, 0xAB710D06, 0xA6322BDF, 0xA2F33668, - 0xBCB4666D, 0xB8757BDA, 0xB5365D03, 0xB1F740B4}; - - /** - * For each byte in the input data, XOR the current CRC value with the byte, then shift the CRC value - * left 8 bits, and XOR the CRC value with the CRC table value for the byte - * - * @param pData pointer to the data to be CRC'd - * @param Length The length of the data to be CRC'd. - * - * @return The CRC32 value of the data. - */ - static inline uint32_t checkCRC32(uint8_t *pData, uint32_t Length) - { - unsigned int nReg; - unsigned int nTemp = 0; - unsigned short i, n; - - nReg = 0xFFFFFFFF; - for (n = 0; n < Length; n++) - { - nReg ^= (unsigned int)pData[n]; - - for (i = 0; i < 4; i++) - { - nTemp = Crc32Table[(unsigned char)((nReg >> 24) & 0xff)]; - nReg <<= 8; - nReg ^= nTemp; - } - } - return nReg; - } - - /** - * It takes a pointer to an array of bytes and the length of the array, and returns the sum of the - * bytes in the array - * - * @param pData The data to be calculated - * @param Lenght The length of the data to be sent. - * - * @return The sum of the bytes in the array. - */ - static inline unsigned char CheckSum(unsigned char *pData, unsigned short Lenght) - { - unsigned short temp = 0; - unsigned short i = 0; - for (i = 0; i < Lenght; i++) - { - temp += pData[i]; - } - return temp & 0XFF; - } - -} - -#endif \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.cpp b/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.cpp deleted file mode 100755 index dda615e..0000000 --- a/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/* - * @Description: - * @Author: L LC @amov - * @Date: 2023-03-01 10:12:58 - * @LastEditors: L LC @amov - * @LastEditTime: 2023-04-11 17:33:42 - * @FilePath: /gimbal-sdk-multi-platform/src/G2/g2_gimbal_driver.cpp - */ - -#include "g2_gimbal_driver.h" -#include "g2_gimbal_crc.h" -#include "string.h" - -/** - * The function creates a new instance of the g2GimbalDriver class, which is a subclass of the - * IamovGimbalBase class - * - * @param _IO The IOStreamBase class that is used to communicate with the gimbal. - */ -g2GimbalDriver::g2GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(_IO) -{ - memset(&rxQueue, 0, sizeof(RING_FIFO_CB_T)); - memset(&txQueue, 0, sizeof(RING_FIFO_CB_T)); - - rxBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T)); - if (rxBuffer == NULL) - { - std::cout << "Receive buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl; - exit(1); - } - txBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T)); - if (txBuffer == NULL) - { - free(rxBuffer); - std::cout << "Send buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl; - exit(1); - } - - Ring_Fifo_init(&rxQueue, sizeof(G2::GIMBAL_FRAME_T), rxBuffer, MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T)); - Ring_Fifo_init(&txQueue, sizeof(G2::GIMBAL_FRAME_T), txBuffer, MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T)); - - parserState = G2::GIMBAL_SERIAL_STATE_IDEL; -} - -/** - * It takes a command, a pointer to a payload, and the size of the payload, and then it puts the - * command, the payload, and the CRC into a ring buffer - * - * @param uint32_t 4 bytes - * @param pPayload pointer to the data to be sent - * @param payloadSize the size of the payload in bytes - * - * @return The number of bytes in the packet. - */ -uint32_t g2GimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize) -{ - uint32_t ret = 0; - G2::GIMBAL_FRAME_T txTemp; - - txTemp.head = G2_SERIAL_HEAD; - txTemp.version = G2_SERIAL_VERSION; - txTemp.len = payloadSize; - txTemp.command = cmd; - txTemp.source = self; - txTemp.target = remote; - memcpy(txTemp.data, pPayload, payloadSize); - txTemp.crc.f16 = G2::checkCrc16((uint8_t *)&txTemp, txTemp.len + G2_PAYLOAD_OFFSET); - memcpy(txTemp.data + payloadSize, txTemp.crc.f8, sizeof(uint16_t)); - - txMutex.lock(); - if (Ring_Fifo_in_cell(&txQueue, &txTemp)) - { - ret = txTemp.len + G2_PAYLOAD_OFFSET + sizeof(uint16_t); - } - txMutex.unlock(); - - return ret; -} - -/** - * > This function is used to get a packet from the receive queue - * - * @param void This is the type of data that will be stored in the queue. - * - * @return A boolean value. - */ -bool g2GimbalDriver::getRxPack(OUT void *pack) -{ - bool state = false; - rxMutex.lock(); - state = Ring_Fifo_out_cell(&rxQueue, pack); - rxMutex.unlock(); - return state; -} - -/** - * The function takes a pointer to a buffer, casts it to a pointer to a G2::GIMBAL_FRAME_T, and then - * checks the command field of the frame. If the command is G2::IAP_COMMAND_BLOCK_END, it locks the - * mutex, and then unlocks it. Otherwise, it prints out the contents of the buffer - * - * @param buf pointer to the data received from the gimbal - */ -void g2GimbalDriver::convert(void *buf) -{ - G2::GIMBAL_FRAME_T *temp; - temp = reinterpret_cast(buf); - switch (temp->command) - { - case G2::IAP_COMMAND_BLOCK_END: - mState.lock(); - updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, - state.abs.roll, state.abs.pitch, state.abs.yaw, - state.fov.x, state.fov.y); - mState.unlock(); - break; - - default: - std::cout << "Undefined frame from G2 : "; - for (uint16_t i = 0; i < temp->len + G2_PAYLOAD_OFFSET + sizeof(uint32_t); i++) - { - printf("%02X ", ((uint8_t *)buf)[i]); - } - std::cout << std::endl; - break; - } -} - -/** - * If the serial port is not busy and is open, then lock the txMutex, get the next byte from the - * txQueue, unlock the txMutex, and send the byte - */ -void g2GimbalDriver::send(void) -{ - if (!IO->isBusy() && IO->isOpen()) - { - bool state = false; - txMutex.lock(); - state = Ring_Fifo_out_cell(&txQueue, &tx); - txMutex.unlock(); - if (state) - { - IO->outPutBytes((uint8_t *)&tx, tx.len + G2_PAYLOAD_OFFSET + sizeof(uint16_t)); - } - } -} - -/** - * The function is called every time a byte is received from the serial port. It parses the byte and - * stores it in a buffer. When the buffer is full, it checks the CRC and if it's correct, it stores the - * buffer in a queue - * - * @param uint8_t unsigned char - * - * @return The parser function is returning a boolean value. - */ -bool g2GimbalDriver::parser(IN uint8_t byte) -{ - bool state = false; - static uint8_t payloadLenghte = 0; - static uint8_t *pRx = NULL; - - switch (parserState) - { - case G2::GIMBAL_SERIAL_STATE_IDEL: - if (byte == G2_SERIAL_HEAD) - { - rx.head = byte; - parserState = G2::GIMBAL_SERIAL_STATE_HEAD_RCV; - } - break; - - case G2::GIMBAL_SERIAL_STATE_HEAD_RCV: - if (byte == G2_SERIAL_VERSION) - { - rx.version = byte; - parserState = G2::GIMBAL_SERIAL_STATE_VERSION_RCV; - } - else - { - rx.head = 0; - parserState = G2::GIMBAL_SERIAL_STATE_IDEL; - } - break; - - case G2::GIMBAL_SERIAL_STATE_VERSION_RCV: - rx.target = byte; - parserState = G2::GIMBAL_SERIAL_STATE_TARGET_RCV; - break; - - case G2::GIMBAL_SERIAL_STATE_TARGET_RCV: - rx.source = byte; - parserState = G2::GIMBAL_SERIAL_STATE_SOURCE_RCV; - break; - - case G2::GIMBAL_SERIAL_STATE_SOURCE_RCV: - rx.len = byte; - parserState = G2::GIMBAL_SERIAL_STATE_LENGHT_RCV; - pRx = rx.data; - payloadLenghte = byte; - break; - - case G2::GIMBAL_SERIAL_STATE_LENGHT_RCV: - rx.command = byte; - parserState = G2::GIMBAL_SERIAL_STATE_DATA_RCV; - break; - - case G2::GIMBAL_SERIAL_STATE_DATA_RCV: - *pRx = byte; - payloadLenghte--; - if (payloadLenghte == 0) - { - parserState = G2::GIMBAL_SERIAL_STATE_CRC_RCV1; - } - break; - - case G2::GIMBAL_SERIAL_STATE_CRC_RCV1: - rx.crc.f8[1] = byte; - parserState = G2::GIMBAL_SERIAL_STATE_END; - break; - - case G2::GIMBAL_SERIAL_STATE_END: - rx.crc.f8[0] = byte; - - if (rx.crc.f16 == G2::checkCrc16((uint8_t *)&rx, G2_PAYLOAD_OFFSET + rx.len)) - { - state = true; - rxMutex.lock(); - Ring_Fifo_in_cell(&rxQueue, &rx); - rxMutex.unlock(); - } - else - { - memset(&rx, 0, sizeof(G2::GIMBAL_FRAME_T)); - } - parserState = G2::GIMBAL_SERIAL_STATE_IDEL; - break; - - default: - parserState = G2::GIMBAL_SERIAL_STATE_IDEL; - break; - } - return state; -} \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.h b/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.h deleted file mode 100755 index d6ac388..0000000 --- a/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.h +++ /dev/null @@ -1,76 +0,0 @@ -/* - * @Description: - * @Author: L LC @amov - * @Date: 2023-03-01 10:02:24 - * @LastEditors: L LC @amov - * @LastEditTime: 2023-03-13 12:29:33 - * @FilePath: \gimbal-sdk-multi-platform\src\G2\g2_gimbal_driver.h - */ -#include "../amov_gimbal.h" -#include "g2_gimbal_struct.h" -#include -#include -#include - -#ifndef __G2_DRIVER_H -#define __G2_DRIVER_H - -extern "C" -{ -#include "Ring_Fifo.h" -} - -class g2GimbalDriver : protected amovGimbal::IamovGimbalBase -{ -private: - G2::GIMBAL_CMD_PARSER_STATE_T parserState; - G2::GIMBAL_FRAME_T rx; - G2::GIMBAL_FRAME_T tx; - - std::mutex rxMutex; - uint8_t *rxBuffer; - RING_FIFO_CB_T rxQueue; - std::mutex txMutex; - uint8_t *txBuffer; - RING_FIFO_CB_T txQueue; - - uint8_t self; - uint8_t remote; - - bool parser(IN uint8_t byte); - void send(void); - - void convert(void *buf); - uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize); - bool getRxPack(OUT void *pack); - -public: - void nodeSet(SET uint32_t _self, SET uint32_t _remote) - { - self = _self; - remote = _remote; - } - - // funtion - uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos); - uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed); - uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed); - uint32_t setGimabalHome(void); - - uint32_t takePic(void); - uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState); - - static amovGimbal::IamovGimbalBase *creat(amovGimbal::IOStreamBase *_IO) - { - return new g2GimbalDriver(_IO); - } - - g2GimbalDriver(amovGimbal::IOStreamBase *_IO); - ~g2GimbalDriver() - { - free(rxBuffer); - free(txBuffer); - } -}; - -#endif diff --git a/gimbal_ctrl/driver/src/G2/g2_gimbal_funtion.cpp b/gimbal_ctrl/driver/src/G2/g2_gimbal_funtion.cpp deleted file mode 100644 index e7b0ca9..0000000 --- a/gimbal_ctrl/driver/src/G2/g2_gimbal_funtion.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/* - * @Description: - * @Author: L LC @amov - * @Date: 2023-03-13 11:58:54 - * @LastEditors: L LC @amov - * @LastEditTime: 2023-03-13 12:31:58 - * @FilePath: \gimbal-sdk-multi-platform\src\G2\g2_gimbal_funtion.cpp - */ -#include "g2_gimbal_driver.h" -#include "g2_gimbal_crc.h" -#include "string.h" - -/** - * It sets the gimbal position. - * - * @param pos the position of the gimbal - * - * @return The return value is the number of bytes written to the buffer. - */ -uint32_t g2GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos) -{ - return 0; -} - -/** - * It takes a struct of type amovGimbal::AMOV_GIMBAL_POS_T and converts it to a struct of type - * G1::GIMBAL_SET_POS_MSG_T - * - * @param speed the speed of the gimbal - * - * @return The return value is the number of bytes written to the buffer. - */ -uint32_t g2GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed) -{ - return 0; -} - -/** - * This function sets the gimbal's follow speed - * - * @param followSpeed the speed of the gimbal - * - * @return The return value is the number of bytes written to the buffer. - */ -uint32_t g2GimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed) -{ - return 0; -} - -/** - * This function sets the gimbal to its home position - * - * @return The return value is the number of bytes written to the buffer. - */ -uint32_t g2GimbalDriver::setGimabalHome(void) -{ - return 0; -} - -/** - * It takes a picture. - * - * @return The return value is the number of bytes written to the serial port. - */ -uint32_t g2GimbalDriver::takePic(void) -{ - return 0; -} - -/** - * The function sets the video state of the gimbal - * - * @param newState The new state of the video. - * - * @return The return value is the number of bytes written to the serial port. - */ -uint32_t g2GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState) -{ - - return 0; -} diff --git a/gimbal_ctrl/driver/src/G2/g2_gimbal_struct.h b/gimbal_ctrl/driver/src/G2/g2_gimbal_struct.h deleted file mode 100755 index 221d0dc..0000000 --- a/gimbal_ctrl/driver/src/G2/g2_gimbal_struct.h +++ /dev/null @@ -1,81 +0,0 @@ -/* - * @Description: - * @Author: L LC @amov - * @Date: 2023-03-01 09:21:57 - * @LastEditors: L LC @amov - * @LastEditTime: 2023-04-18 10:13:23 - * @FilePath: /gimbal-sdk-multi-platform/src/G2/g2_gimbal_struct.h - */ - -#ifndef G2_GIMBAL_STRUCT_H -#define G2_GIMBAL_STRUCT_H - -#include - -namespace G2 -{ - -#define G2_MAX_GIMBAL_PAYLOAD 64 -#define G2_PAYLOAD_OFFSET 6 -#define G2_SCALE_FACTOR 0.01f -#define G2_SERIAL_HEAD 0XAF -#define G2_SERIAL_VERSION 0X02 - - typedef enum - { - IAP_COMMAND_JUMP = 80, - IAP_COMMAND_FLASH_ERASE, - IAP_COMMAND_BOLCK_INFO, - IAP_COMMAND_BLOCK_WRITE, - IAP_COMMAND_SOFT_INFO, - IAP_COMMAND_HARDWARE_INFO, - IAP_COMMAND_BLOCK_START, - IAP_COMMAND_BLOCK_END = 117, - } GIMBAL_CMD_T; - - typedef enum - { - IAP_STATE_FAILD = 0, - IAP_STATE_SUCCEED, - IAP_STATE_READY, - IAP_STATE_FIRMWARE_BROKE, - IAP_STATE_JUMP_FAILD, - IAP_STATE_ADDR_ERR, - IAP_STATE_CRC_ERR, - IAP_STATE_WRITE_ERR, - IAP_STATE_WRITE_TIMEOUT, - } GIMBAL_IAP_STATE_T; - - typedef enum - { - GIMBAL_SERIAL_STATE_IDEL = 0, - GIMBAL_SERIAL_STATE_HEAD_RCV, - GIMBAL_SERIAL_STATE_VERSION_RCV, - GIMBAL_SERIAL_STATE_TARGET_RCV, - GIMBAL_SERIAL_STATE_SOURCE_RCV, - GIMBAL_SERIAL_STATE_LENGHT_RCV, - GIMBAL_SERIAL_STATE_DATA_RCV, - GIMBAL_SERIAL_STATE_CRC_RCV1, - GIMBAL_SERIAL_STATE_END, - } GIMBAL_CMD_PARSER_STATE_T; - -#pragma pack(1) - typedef struct - { - uint8_t head; - uint8_t version; - uint8_t target; - uint8_t source; - uint8_t len; - uint8_t command; - uint8_t data[G2_MAX_GIMBAL_PAYLOAD]; - union - { - uint8_t f8[2]; - uint16_t f16; - } crc; - } GIMBAL_FRAME_T; -#pragma pack(0) - -} -#endif \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp index fc1045d..4262cf2 100755 --- a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp +++ b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:06 * @LastEditors: L LC @amov - * @LastEditTime: 2023-04-11 17:29:58 + * @LastEditTime: 2023-07-24 12:03:44 * @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_driver.cpp */ #include "Q10f_gimbal_driver.h" @@ -16,27 +16,10 @@ * * @param _IO The IOStreamBase object that will be used to communicate with the gimbal. */ -Q10fGimbalDriver::Q10fGimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(_IO) +Q10fGimbalDriver::Q10fGimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::amovGimbalBase(_IO) { - memset(&rxQueue, 0, sizeof(RING_FIFO_CB_T)); - memset(&txQueue, 0, sizeof(RING_FIFO_CB_T)); - - rxBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T)); - if (rxBuffer == NULL) - { - std::cout << "Receive buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl; - exit(1); - } - txBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T)); - if (txBuffer == NULL) - { - free(rxBuffer); - std::cout << "Send buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl; - exit(1); - } - - Ring_Fifo_init(&rxQueue, sizeof(Q10f::GIMBAL_FRAME_T), rxBuffer, MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T)); - Ring_Fifo_init(&txQueue, sizeof(Q10f::GIMBAL_FRAME_T), txBuffer, MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T)); + rxQueue = new fifoRing(sizeof(Q10f::GIMBAL_FRAME_T), MAX_QUEUE_SIZE); + txQueue = new fifoRing(sizeof(Q10f::GIMBAL_FRAME_T), MAX_QUEUE_SIZE); parserState = Q10f::GIMBAL_SERIAL_STATE_IDLE; @@ -77,32 +60,14 @@ uint32_t Q10fGimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payl } txTemp.len = payloadSize; - txMutex.lock(); - if (Ring_Fifo_in_cell(&txQueue, &txTemp)) + if (txQueue->inCell(&txTemp)) { ret = payloadSize + sizeof(uint32_t) + sizeof(uint8_t); } - txMutex.unlock(); return ret; } -/** - * > This function is used to get a packet from the receive queue - * - * @param void This is the type of data that will be stored in the queue. - * - * @return A boolean value. - */ -bool Q10fGimbalDriver::getRxPack(OUT void *pack) -{ - bool state = false; - rxMutex.lock(); - state = Ring_Fifo_out_cell(&rxQueue, pack); - rxMutex.unlock(); - return state; -} - void Q10fGimbalDriver::convert(void *buf) { Q10f::GIMBAL_FRAME_T *temp; @@ -116,7 +81,7 @@ void Q10fGimbalDriver::convert(void *buf) state.abs.yaw = tempPos->yawIMUAngle * Q10F_SCALE_FACTOR_ANGLE; state.abs.roll = tempPos->rollIMUAngle * Q10F_SCALE_FACTOR_ANGLE; state.abs.pitch = tempPos->pitchIMUAngle * Q10F_SCALE_FACTOR_ANGLE; - state.rel.yaw = tempPos->rollStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED; + state.rel.yaw = tempPos->yawStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED; state.rel.roll = tempPos->rollStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED; state.rel.pitch = tempPos->pitchStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED; updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, @@ -136,32 +101,9 @@ void Q10fGimbalDriver::convert(void *buf) } } -/** - * The function is called by the main thread to send a command to the gimbal. - * - * The function first checks to see if the serial port is busy and if it is open. If it is not busy and - * it is open, the function locks the txMutex and then checks to see if there is a command in the - * txQueue. If there is a command in the txQueue, the function copies the command to the tx buffer and - * then unlocks the txMutex. The function then sends the command to the gimbal. - * - * The txQueue is a ring buffer that holds commands that are waiting to be sent to the gimbal. The - * txQueue is a ring buffer because the gimbal can only process one command at a time. If the gimbal is - * busy processing a command, the command will be placed in the txQueue and sent to the gimbal when the - * gimbal is ready to receive the command. - */ -void Q10fGimbalDriver::send(void) +uint32_t Q10fGimbalDriver::calPackLen(void *pack) { - if (!IO->isBusy() && IO->isOpen()) - { - bool state = false; - txMutex.lock(); - state = Ring_Fifo_out_cell(&txQueue, &tx); - txMutex.unlock(); - if (state) - { - IO->outPutBytes((uint8_t *)&tx, tx.len + Q10F_PAYLOAD_OFFSET + sizeof(uint8_t)); - } - } + return ((Q10f::GIMBAL_FRAME_T *)pack)->len + Q10F_PAYLOAD_OFFSET + sizeof(uint8_t); } /** @@ -238,9 +180,7 @@ bool Q10fGimbalDriver::parser(IN uint8_t byte) if (byte == suncheck) { state = true; - rxMutex.lock(); - Ring_Fifo_in_cell(&rxQueue, &rx); - rxMutex.unlock(); + rxQueue->inCell(&rx); } else { diff --git a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h index 0def5a1..38a849a 100755 --- a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h +++ b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h @@ -7,6 +7,7 @@ * @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_driver.h */ #include "../amov_gimbal.h" +#include "../amov_gimbal_private.h" #include "Q10f_gimbal_struct.h" #include #include @@ -15,31 +16,16 @@ #ifndef __Q10F_DRIVER_H #define __Q10F_DRIVER_H -extern "C" -{ -#include "Ring_Fifo.h" -} - -class Q10fGimbalDriver : protected amovGimbal::IamovGimbalBase +class Q10fGimbalDriver : protected amovGimbal::amovGimbalBase { private: Q10f::GIMBAL_SERIAL_STATE_T parserState; Q10f::GIMBAL_FRAME_T rx; - Q10f::GIMBAL_FRAME_T tx; - - std::mutex rxMutex; - uint8_t *rxBuffer; - RING_FIFO_CB_T rxQueue; - std::mutex txMutex; - uint8_t *txBuffer; - RING_FIFO_CB_T txQueue; bool parser(IN uint8_t byte); - void send(void); - void convert(void *buf); uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize); - bool getRxPack(OUT void *pack); + uint32_t calPackLen(void *pack); public: // funtions @@ -55,17 +41,12 @@ public: uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState); // builds - static amovGimbal::IamovGimbalBase *creat(amovGimbal::IOStreamBase *_IO) + static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO) { return new Q10fGimbalDriver(_IO); } Q10fGimbalDriver(amovGimbal::IOStreamBase *_IO); - ~Q10fGimbalDriver() - { - free(rxBuffer); - free(txBuffer); - } }; #endif diff --git a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp index dff1e8d..4406d07 100755 --- a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp +++ b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2023-03-02 10:00:52 * @LastEditors: L LC @amov - * @LastEditTime: 2023-03-29 11:47:18 + * @LastEditTime: 2023-07-24 14:22:57 * @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_funtion.cpp */ #include "Q10f_gimbal_driver.h" diff --git a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h index c5061d8..fc701d4 100755 --- a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h +++ b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:07 * @LastEditors: L LC @amov - * @LastEditTime: 2023-03-28 18:15:47 + * @LastEditTime: 2023-07-24 11:51:59 * @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_struct.h */ #ifndef Q10F_GIMBAL_STRUCT_H diff --git a/gimbal_ctrl/driver/src/amov_gimabl.cpp b/gimbal_ctrl/driver/src/amov_gimbal.cpp similarity index 52% rename from gimbal_ctrl/driver/src/amov_gimabl.cpp rename to gimbal_ctrl/driver/src/amov_gimbal.cpp index cc0d7f3..a0e9190 100755 --- a/gimbal_ctrl/driver/src/amov_gimabl.cpp +++ b/gimbal_ctrl/driver/src/amov_gimbal.cpp @@ -3,14 +3,15 @@ * @Author: L LC @amov * @Date: 2022-10-28 11:54:11 * @LastEditors: L LC @amov - * @LastEditTime: 2023-04-11 18:13:25 - * @FilePath: /gimbal-sdk-multi-platform/src/amov_gimabl.cpp + * @LastEditTime: 2023-08-17 11:57:11 + * @FilePath: /gimbal-sdk-multi-platform/src/amov_gimbal.cpp */ -#include "amov_gimbal.h" +// #include "amov_gimbal.h" +#include "amov_gimbal_private.h" #include "g1_gimbal_driver.h" -#include "g2_gimbal_driver.h" #include "Q10f_gimbal_driver.h" +#include "AT10_gimbal_driver.h" #include #include @@ -24,23 +25,24 @@ typedef enum AMOV_GIMBAL_TYPE_G1 = 1, AMOV_GIMBAL_TYPE_G2, AMOV_GIMBAL_TYPE_Q10, + AMOV_GIMBAL_TYPE_AT10, } AMOV_GIMBAL_TYPE_T; namespace amovGimbal { - typedef amovGimbal::IamovGimbalBase *(*createCallback)(amovGimbal::IOStreamBase *_IO); + typedef amovGimbal::amovGimbalBase *(*createCallback)(amovGimbal::IOStreamBase *_IO); typedef std::map callbackMap; std::map amovGimbalTypeList = { {"G1", AMOV_GIMBAL_TYPE_G1}, - {"G2", AMOV_GIMBAL_TYPE_G2}, - {"Q10f", AMOV_GIMBAL_TYPE_Q10}}; + {"Q10f", AMOV_GIMBAL_TYPE_Q10}, + {"AT10", AMOV_GIMBAL_TYPE_AT10}}; callbackMap amovGimbals = { {"G1", g1GimbalDriver::creat}, - {"G2", g2GimbalDriver::creat}, - {"Q10f", Q10fGimbalDriver::creat}}; + {"Q10f", Q10fGimbalDriver::creat}, + {"AT10", AT10GimbalDriver::creat}}; } /* The amovGimbalCreator class is a factory class that creates an instance of the amovGimbal class */ @@ -48,7 +50,7 @@ namespace amovGimbal class amovGimbalCreator { public: - static amovGimbal::IamovGimbalBase *createAmovGimbal(const std::string &type, amovGimbal::IOStreamBase *_IO) + static amovGimbal::amovGimbalBase *createAmovGimbal(const std::string &type, amovGimbal::IOStreamBase *_IO) { amovGimbal::callbackMap::iterator temp = amovGimbal::amovGimbals.find(type); @@ -77,13 +79,71 @@ private: ~amovGimbalCreator(); }; +/** + * This is a constructor for the amovGimbalBase class that initializes its parent classes with an + * IOStreamBase object. + * + * @param _IO _IO is a pointer to an object of type amovGimbal::IOStreamBase, which is the base class + * for input/output streams used by the amovGimbal class. This parameter is passed to the constructor + * of amovGimbalBase, which is a derived class of I + */ +amovGimbal::amovGimbalBase::amovGimbalBase(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(), amovGimbal::PamovGimbalBase(_IO) +{ +} + +/** + * The function is a destructor that sleeps for 50 milliseconds and closes an IO object. + */ +amovGimbal::amovGimbalBase::~amovGimbalBase() +{ + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + IO->close(); +} + +/** + * This function retrieves a packet from a ring buffer queue and returns a boolean value indicating + * whether the operation was successful or not. + * + * @param void void is a keyword in C++ that represents the absence of a type. In this function, it is + * used to indicate that the function does not return any value. + * + * @return a boolean value, which indicates whether or not a data packet was successfully retrieved + * from a ring buffer queue. + */ +bool amovGimbal::amovGimbalBase::getRxPack(OUT void *pack) +{ + bool state = false; + state = rxQueue->outCell(pack); + return state; +} + +/** + * This function sends data from a buffer to an output device if it is not busy and open. + */ +void amovGimbal::amovGimbalBase::send(void) +{ + uint8_t tempBuffer[MAX_PACK_SIZE]; + + if (!IO->isBusy() && IO->isOpen()) + { + bool state = false; + + state = txQueue->outCell(&tempBuffer); + + if (state) + { + IO->outPutBytes((uint8_t *)&tempBuffer, calPackLen(tempBuffer)); + } + } +} + /** * "If the input byte is available, then parse it." * * The function is a loop that runs forever. It calls the IO->inPutByte() function to get a byte from * the serial port. If the byte is available, then it calls the parser() function to parse the byte */ -void amovGimbal::IamovGimbalBase::parserLoop(void) +void amovGimbal::amovGimbalBase::parserLoop(void) { uint8_t temp; @@ -96,7 +156,7 @@ void amovGimbal::IamovGimbalBase::parserLoop(void) } } -void amovGimbal::IamovGimbalBase::sendLoop(void) +void amovGimbal::amovGimbalBase::sendLoop(void) { while (1) { @@ -104,7 +164,7 @@ void amovGimbal::IamovGimbalBase::sendLoop(void) } } -void amovGimbal::IamovGimbalBase::mainLoop(void) +void amovGimbal::amovGimbalBase::mainLoop(void) { uint8_t tempBuffer[MAX_PACK_SIZE]; @@ -112,52 +172,66 @@ void amovGimbal::IamovGimbalBase::mainLoop(void) { if (getRxPack(tempBuffer)) { + msgCustomCallback(tempBuffer); convert(tempBuffer); } } } +void amovGimbal::amovGimbalBase::stackStart(void) +{ + if (!this->IO->isOpen()) + { + this->IO->open(); + } + std::thread parserLoop(&amovGimbalBase::parserLoop, this); + std::thread sendLoop(&amovGimbalBase::sendLoop, this); + parserLoop.detach(); + sendLoop.detach(); +} /** * It starts two threads, one for reading data from the serial port and one for sending data to the * serial port */ -void amovGimbal::IamovGimbalBase::startStack(void) +void amovGimbal::gimbal::startStack(void) { - if (!IO->isOpen()) - { - IO->open(); - } - - std::thread mainLoop(&IamovGimbalBase::parserLoop, this); - std::thread sendLoop(&IamovGimbalBase::sendLoop, this); - mainLoop.detach(); - sendLoop.detach(); + ((amovGimbalBase *)(this->dev))->stackStart(); } +void amovGimbal::gimbal::setParserCallback(amovGimbal::pStateInvoke callback) +{ + ((amovGimbalBase *)(this->dev))->updateGimbalStateCallback = callback; +} + +void amovGimbal::amovGimbalBase::parserStart(pStateInvoke callback) +{ + this->updateGimbalStateCallback = callback; + + std::thread mainLoop(&amovGimbalBase::mainLoop, this); + + mainLoop.detach(); +} /** * The function creates a thread that runs the mainLoop function */ -void amovGimbal::IamovGimbalBase::parserAuto(pStateInvoke callback) +void amovGimbal::gimbal::parserAuto(pStateInvoke callback) { - this->updateGimbalStateCallback = callback; - std::thread mainLoop(&IamovGimbalBase::mainLoop, this); - mainLoop.detach(); + ((amovGimbalBase *)(this->dev))->parserStart(callback); } -amovGimbal::AMOV_GIMBAL_STATE_T amovGimbal::IamovGimbalBase::getGimabalState(void) +void amovGimbal::gimbal::setMsgCallback(pMsgInvoke callback) { - mState.lock(); - AMOV_GIMBAL_STATE_T temp = state; - mState.unlock(); + ((amovGimbalBase *)(this->dev))->msgCustomCallback = callback; +} + +amovGimbal::AMOV_GIMBAL_STATE_T amovGimbal::gimbal::getGimabalState(void) +{ + ((amovGimbalBase *)(this->dev))->mState.lock(); + AMOV_GIMBAL_STATE_T temp = ((amovGimbalBase *)(this->dev))->state; + ((amovGimbalBase *)(this->dev))->mState.unlock(); return temp; } -amovGimbal::IamovGimbalBase::~IamovGimbalBase() -{ - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - IO->close(); -} - /** * Default implementation of interface functions, not pure virtual functions for ease of extension. */ @@ -211,6 +285,21 @@ uint32_t amovGimbal::IamovGimbalBase::setVideo(const amovGimbal::AMOV_GIMBAL_VID return 0; } +uint32_t amovGimbal::IamovGimbalBase::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion, const AMOV_GIMBAL_VELOCITY_T &speed, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData) +{ + return 0; +} + +uint32_t amovGimbal::IamovGimbalBase::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, const AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData) +{ + return 0; +} + +uint32_t amovGimbal::IamovGimbalBase::extensionFuntions(void *cmd) +{ + return 0; +} + /** * The function creates a new gimbal object, which is a pointer to a new amovGimbal object, which is a * pointer to a new Gimbal object, which is a pointer to a new IOStreamBase object diff --git a/gimbal_ctrl/driver/src/amov_gimbal.h b/gimbal_ctrl/driver/src/amov_gimbal.h index 52a42f1..442f91f 100755 --- a/gimbal_ctrl/driver/src/amov_gimbal.h +++ b/gimbal_ctrl/driver/src/amov_gimbal.h @@ -3,8 +3,8 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:34:26 * @LastEditors: L LC @amov - * @LastEditTime: 2023-04-18 11:42:05 - * @FilePath: /spirecv-gimbal-sdk/gimbal_ctrl/driver/src/amov_gimbal.h + * @LastEditTime: 2023-08-16 22:21:28 + * @FilePath: /gimbal-sdk-multi-platform/src/amov_gimbal.h */ #ifndef AMOV_GIMBAL_H @@ -14,14 +14,9 @@ #include #include -#include -#include -#include - #include "amov_gimbal_struct.h" -#define MAX_QUEUE_SIZE 50 - +#define MAX_QUEUE_SIZE 100 namespace amovGimbal { #define IN @@ -33,6 +28,10 @@ namespace amovGimbal double &fovX, double &fovY) { } + static inline void idleMsgCallback(void *) + { + } + // Control data input and output class IOStreamBase @@ -50,43 +49,14 @@ namespace amovGimbal virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght) = 0; }; + // Device interface class IamovGimbalBase { - protected: - AMOV_GIMBAL_STATE_T state; - std::mutex mState; - IOStreamBase *IO; - pStateInvoke updateGimbalStateCallback; - - virtual bool parser(IN uint8_t byte) = 0; - virtual void send(void) = 0; - virtual void convert(void *buf) = 0; - virtual uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize) = 0; - virtual bool getRxPack(OUT void *pack) = 0; - - void parserLoop(void); - void sendLoop(void); - void mainLoop(void); - public: - IamovGimbalBase(SET IOStreamBase *_IO) - { - IO = _IO; - } - virtual ~IamovGimbalBase(); - - void setParserCallback(pStateInvoke callback) - { - this->updateGimbalStateCallback = callback; - } - - // Protocol stack function items - virtual void startStack(void); - virtual void parserAuto(pStateInvoke callback = idleCallback); - virtual void nodeSet(SET uint32_t _self, SET uint32_t _remote); - + IamovGimbalBase() {} + virtual ~IamovGimbalBase() {} // functions - virtual AMOV_GIMBAL_STATE_T getGimabalState(void); + virtual void nodeSet(SET uint32_t _self, SET uint32_t _remote); virtual uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos); virtual uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed); virtual uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed); @@ -96,6 +66,9 @@ namespace amovGimbal virtual uint32_t setGimbalROI(const AMOV_GIMBAL_ROI_T area); virtual uint32_t takePic(void); virtual uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState); + virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion, const AMOV_GIMBAL_VELOCITY_T &speed, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData); + virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, const AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData); + virtual uint32_t extensionFuntions(void *cmd); }; class gimbal @@ -105,7 +78,17 @@ namespace amovGimbal IOStreamBase *IO; public: + // Instantiated device handle IamovGimbalBase *dev; + + // Protocol stack function items + void startStack(void); + void parserAuto(pStateInvoke callback = idleCallback); + void setParserCallback(pStateInvoke callback); + void setMsgCallback(pMsgInvoke callback); + + AMOV_GIMBAL_STATE_T getGimabalState(void); + std::string name() { return typeName; diff --git a/gimbal_ctrl/driver/src/amov_gimbal_private.h b/gimbal_ctrl/driver/src/amov_gimbal_private.h new file mode 100644 index 0000000..aace061 --- /dev/null +++ b/gimbal_ctrl/driver/src/amov_gimbal_private.h @@ -0,0 +1,79 @@ +/* + * @Description : + * @Author : Aiyangsky + * @Date : 2023-05-13 10:39:20 + * @LastEditors: L LC @amov + * @LastEditTime: 2023-08-16 22:30:53 + * @FilePath: /gimbal-sdk-multi-platform/src/amov_gimbal_private.h + */ +#ifndef __AMOV_GIMABL_PRIVATE_H +#define __AMOV_GIMABL_PRIVATE_H + +#include +#include +#include + +#include +#include +#include + +#include "amov_gimbal.h" + +#include "Ring_Fifo.h" +#include "amov_tool.h" +namespace amovGimbal +{ + class PamovGimbalBase + { + public: + AMOV_GIMBAL_STATE_T state; + std::mutex mState; + IOStreamBase *IO; + pStateInvoke updateGimbalStateCallback; + pMsgInvoke msgCustomCallback = idleMsgCallback; + + fifoRing *rxQueue; + fifoRing *txQueue; + + PamovGimbalBase(SET IOStreamBase *_IO) + { + IO = _IO; + } + virtual ~PamovGimbalBase() + { + if (txQueue != nullptr) + { + delete txQueue; + } + if (rxQueue != nullptr) + { + delete rxQueue; + } + } + }; + + class amovGimbalBase : public IamovGimbalBase, public PamovGimbalBase + { + public: + virtual uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize) = 0; + virtual bool parser(IN uint8_t byte) = 0; + virtual void convert(void *buf) = 0; + virtual uint32_t calPackLen(void *pack) = 0; + + virtual void send(void); + virtual bool getRxPack(OUT void *pack); + + virtual void parserLoop(void); + virtual void sendLoop(void); + virtual void mainLoop(void); + + virtual void stackStart(void); + virtual void parserStart(amovGimbal::pStateInvoke callback); + + public: + amovGimbalBase(IOStreamBase *_IO); + virtual ~amovGimbalBase(); + }; +} + +#endif \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/amov_gimbal_struct.h b/gimbal_ctrl/driver/src/amov_gimbal_struct.h index f7bebae..a7451b0 100755 --- a/gimbal_ctrl/driver/src/amov_gimbal_struct.h +++ b/gimbal_ctrl/driver/src/amov_gimbal_struct.h @@ -3,20 +3,21 @@ * @Author: L LC @amov * @Date: 2022-10-31 11:56:43 * @LastEditors: L LC @amov - * @LastEditTime: 2023-04-18 10:12:33 + * @LastEditTime: 2023-08-16 22:21:46 * @FilePath: /gimbal-sdk-multi-platform/src/amov_gimbal_struct.h */ #include -#ifndef __AMOV_GIMABL_STRUCT_H -#define __AMOV_GIMABL_STRUCT_H +#ifndef __AMOV_GIMABL_STRUCT_H +#define __AMOV_GIMABL_STRUCT_H namespace amovGimbal { typedef void (*pStateInvoke)(double &frameAngleRoll, double &frameAnglePitch, double &frameAngleYaw, double &imuAngleRoll, double &imuAnglePitch, double &imuAngleYaw, double &fovX, double &fovY); + typedef void (*pMsgInvoke)(void *msg); typedef enum { @@ -48,8 +49,7 @@ namespace amovGimbal { double x; double y; - }AMOV_GIMBAL_FOV_T; - + } AMOV_GIMBAL_FOV_T; typedef struct { @@ -69,6 +69,21 @@ namespace amovGimbal uint32_t width; } AMOV_GIMBAL_ROI_T; + typedef struct + { + double q0; + double q1; + double q2; + double q3; + } AMOV_GIMBAL_QUATERNION_T; + + typedef struct + { + double x; + double y; + double z; + } AMOV_GIMBAL_VELOCITY_T; + } // namespace amovGimbal #endif diff --git a/gimbal_ctrl/driver/src/amov_tool.h b/gimbal_ctrl/driver/src/amov_tool.h new file mode 100644 index 0000000..e165df6 --- /dev/null +++ b/gimbal_ctrl/driver/src/amov_tool.h @@ -0,0 +1,29 @@ +/* + * @Description: + * @Author: L LC @amov + * @Date: 2023-07-31 18:30:33 + * @LastEditors: L LC @amov + * @LastEditTime: 2023-07-31 18:55:18 + * @FilePath: /gimbal-sdk-multi-platform/src/amov_tool.h + */ + +namespace amovGimbalTools +{ + static inline unsigned short conversionBigLittle(unsigned short value) + { + unsigned short temp = 0; + temp |= ((value >> 8) & 0X00FF); + temp |= ((value << 8) & 0XFF00); + return temp; + } + + static inline unsigned int conversionBigLittle(unsigned int value) + { + unsigned int temp = 0; + temp |= ((value << 24) & 0XFF000000); + temp |= ((value << 8) & 0X00FF0000); + temp |= ((value >> 8) & 0X0000FF00); + temp |= ((value << 24) & 0X000000FF); + return temp; + } +} diff --git a/gimbal_ctrl/sv_gimbal.cpp b/gimbal_ctrl/sv_gimbal.cpp index 233ef41..d01406c 100644 --- a/gimbal_ctrl/sv_gimbal.cpp +++ b/gimbal_ctrl/sv_gimbal.cpp @@ -15,6 +15,28 @@ #include #include #include +namespace sv +{ + std::map Gimbal::IOList; + std::mutex Gimbal::IOListMutex; + + std::map gimbaltypeList = + { + {GimbalType::G1, "G1"}, + {GimbalType::Q10f, "Q10f"}, + {GimbalType::AT10, "AT10"}}; + + std::string &cvGimbalType2Str(const GimbalType &type) + { + std::map::iterator temp = gimbaltypeList.find(type); + if (temp != gimbaltypeList.end()) + { + return (temp->second); + } + throw "Error: Unsupported gimbal device type!!!!"; + exit(-1); + } +} /** * This function sets the serial port for a Gimbal object. @@ -99,9 +121,65 @@ void sv::Gimbal::setNetPort(const int &port) void sv::Gimbal::setStateCallback(sv::PStateInvoke callback) { amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; - pdevTemp->dev->setParserCallback(callback); + pdevTemp->setParserCallback(callback); } +/** + * The function `creatIO` creates an IO object for a gimbal device and adds it to a list of IO objects + * if it doesn't already exist. + * + * @param dev A pointer to an instance of the `Gimbal` class. + * @param IO The parameter "IO" is a pointer to an object that represents the input/output (IO) device + * for the gimbal. It is used to establish a connection with the gimbal and perform communication + * operations. + * + * @return a boolean value. + */ +void *sv::Gimbal::creatIO(sv::Gimbal *dev) +{ + IOListMutex.lock(); + std::map::iterator list = IOList.find(dev->m_serial_port); + std::pair key("NULL", nullptr); + + if (list == IOList.end()) + { + if (dev->m_gimbal_link == GimbalLink::SERIAL) + { + UART *ser; + ser = new UART(dev->m_serial_port, + (uint32_t)dev->m_serial_baud_rate, + serial::Timeout::simpleTimeout(dev->m_serial_timeout), + (serial::bytesize_t)dev->m_serial_byte_size, + (serial::parity_t)dev->m_serial_parity, + (serial::stopbits_t)dev->m_serial_stopbits, + (serial::flowcontrol_t)dev->m_serial_flowcontrol); + key.first = dev->m_serial_port; + key.second = (void *)ser; + IOList.insert(key); + } + else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_TCP) + { + } + else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_UDP) + { + } + else + { + std::cout << "Error: gimbal IO has opened!!!" << std::endl; + } + } + IOListMutex.unlock(); + + return key.second; +} + +void sv::Gimbal::removeIO(sv::Gimbal *dev) +{ + IOListMutex.lock(); + IOList.erase(dev->m_serial_port); + + IOListMutex.unlock(); +} /** * The function opens a communication interface with a gimbal device and sets up a parser to handle * incoming data. @@ -115,53 +193,22 @@ void sv::Gimbal::setStateCallback(sv::PStateInvoke callback) */ bool sv::Gimbal::open(PStateInvoke callback) { - if (this->m_gimbal_link == GimbalLink::SERIAL) - { - this->IO = new UART(this->m_serial_port, - (uint32_t)this->m_serial_baud_rate, - serial::Timeout::simpleTimeout(this->m_serial_timeout), - (serial::bytesize_t)this->m_serial_byte_size, - (serial::parity_t)this->m_serial_parity, - (serial::stopbits_t)this->m_serial_stopbits, - (serial::flowcontrol_t)this->m_serial_flowcontrol); - } - // Subsequent additions - else if (this->m_gimbal_link == sv::GimbalLink::ETHERNET_TCP) - { - return false; - } - else if (this->m_gimbal_link == sv::GimbalLink::ETHERNET_UDP) - { - return false; - } - else - { - throw "Error: Unsupported communication interface class!!!"; - return false; - } - std::string driverName; - switch (this->m_gimbal_type) - { - case sv::GimbalType::G1: - driverName = "G1"; - break; - case sv::GimbalType::Q10f: - driverName = "Q10f"; - break; + bool ret = false; - default: - throw "Error: Unsupported driver!!!"; - return false; - break; + this->IO = creatIO(this); + if (this->IO != nullptr) + { + std::string driverName; + driverName = sv::cvGimbalType2Str(this->m_gimbal_type); + this->dev = new amovGimbal::gimbal(driverName, (amovGimbal::IOStreamBase *)this->IO); + amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; + pdevTemp->startStack(); + pdevTemp->parserAuto(callback); + + ret = true; } - this->dev = new amovGimbal::gimbal(driverName, (amovGimbal::IOStreamBase *)this->IO); - amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; - - pdevTemp->dev->startStack(); - pdevTemp->dev->parserAuto(callback); - - return true; + return ret; } /** @@ -333,7 +380,7 @@ int sv::Gimbal::getVideoState() amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; int ret; amovGimbal::AMOV_GIMBAL_STATE_T state; - state = pdevTemp->dev->getGimabalState(); + state = pdevTemp->getGimabalState(); if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE) { ret = 1; @@ -406,6 +453,7 @@ void sv::Gimbal::setAngleRateEuler(double roll_rate, double pitch_rate, double y sv::Gimbal::~Gimbal() { + removeIO(this); delete (amovGimbal::gimbal *)this->dev; delete (amovGimbal::IOStreamBase *)this->IO; } diff --git a/include/sv_algorithm_base.h b/include/sv_algorithm_base.h index 77685bd..05b6e12 100644 --- a/include/sv_algorithm_base.h +++ b/include/sv_algorithm_base.h @@ -177,3 +177,4 @@ protected: } #endif + diff --git a/include/sv_gimbal.h b/include/sv_gimbal.h index e63bc18..445f257 100644 --- a/include/sv_gimbal.h +++ b/include/sv_gimbal.h @@ -10,6 +10,10 @@ #define __SV_GIMBAL__ #include +#include +#include +#include +#include namespace sv { @@ -21,7 +25,8 @@ namespace sv enum class GimbalType { G1, - Q10f + Q10f, + AT10, }; enum class GimbalLink { @@ -95,6 +100,10 @@ namespace sv GimbalType m_gimbal_type; GimbalLink m_gimbal_link; + static std::map IOList; + static std::mutex IOListMutex; + static void *creatIO(Gimbal *dev); + static void removeIO(Gimbal *dev); public: //! Constructor /*! diff --git a/include/sv_mot.h b/include/sv_mot.h index 9ed81df..4fbac61 100644 --- a/include/sv_mot.h +++ b/include/sv_mot.h @@ -56,6 +56,7 @@ public: int hits; int misses; int frame_id = 0; + int category_id; bool tentative; std::vector features; Eigen::Matrix mean; @@ -70,6 +71,7 @@ public: std::pair, Eigen::Matrix > initiate(Eigen::Vector4d &bbox); std::pair, Eigen::Matrix > update(Eigen::Matrix mean, Eigen::Matrix covariances, Box &box); std::pair, Eigen::Matrix > predict(Eigen::Matrix mean, Eigen::Matrix covariances); + std::pair, Eigen::Matrix > project(Eigen::Matrix mean, Eigen::Matrix covariances); private: Eigen::Matrix _F; Eigen::Matrix _H; @@ -88,7 +90,10 @@ public: private: double _iou(Tracklet &tracklet, Box &box); std::vector> _hungarian(std::vector> costMatrix); - bool _augment(const std::vector>& costMatrix, int row, std::vector& rowMatch, std::vector& colMatch, std::vector& visited); + double _findMin(const std::vector& vec); + void _subtractMinFromRows(std::vector>& costMatrix); + void _subtractMinFromCols(std::vector>& costMatrix); + //bool _augment(const std::vector>& costMatrix, int row, std::vector& rowMatch, std::vector& colMatch, std::vector& visited); double _iou_threshold; int _max_age; diff --git a/include/sv_veri_det.h b/include/sv_veri_det.h new file mode 100644 index 0000000..d372919 --- /dev/null +++ b/include/sv_veri_det.h @@ -0,0 +1,36 @@ +#ifndef __SV_VERI_DET__ +#define __SV_VERI_DET__ + +#include "sv_core.h" +#include +#include +#include +#include +#include + + +namespace sv { + +class VeriDetectorCUDAImpl; + +class VeriDetector : public LandingMarkerDetectorBase +{ +public: + VeriDetector(); + ~VeriDetector(); + + void detect(cv::Mat img1_, cv::Mat img2_, TargetsInFrame &tgts_); + +protected: + bool setupImpl(); + void roiCNN( + std::vector& input_rois_, + std::vector& output_labels_ + ); + + VeriDetectorCUDAImpl* _cuda_impl; +}; + + +} +#endif diff --git a/include/sv_video_base.h b/include/sv_video_base.h index f026ea3..2731947 100644 --- a/include/sv_video_base.h +++ b/include/sv_video_base.h @@ -12,6 +12,9 @@ #include #include // for sockaddr_in +#include +#include + #define SV_RAD2DEG 57.2957795 // #define X86_PLATFORM // #define JETSON_PLATFORM @@ -358,7 +361,15 @@ protected: void _run(); bool _is_running; + +// new mutex + std::mutex _frame_mutex; + std::condition_variable_any _frame_empty; + +//old flag bool _is_updated; + + std::thread _tt; cv::VideoCapture _cap; cv::Mat _frame; @@ -377,6 +388,21 @@ protected: double _exposure; }; +void drawTargetsInFrame( + cv::Mat& img_, + const TargetsInFrame& tgts_, + int aruco_track_id, + bool with_all=true, + bool with_category=false, + bool with_tid=false, + bool with_seg=false, + bool with_box=false, + bool with_ell=false, + bool with_aruco=false, + bool with_yaw=false +); + + void drawTargetsInFrame( cv::Mat& img_, @@ -397,3 +423,4 @@ void list_dir(std::string dir, std::vector& files, std::string suff } #endif + diff --git a/include/sv_world.h b/include/sv_world.h index 29ccde5..4122f9d 100644 --- a/include/sv_world.h +++ b/include/sv_world.h @@ -7,6 +7,7 @@ #include "sv_sot.h" #include "sv_mot.h" #include "sv_color_line.h" +#include "sv_veri_det.h" #include "sv_video_input.h" #include "sv_video_output.h" diff --git a/samples/calib/calibrate_camera_charuco.cpp b/samples/calib/calibrate_camera_charuco.cpp index d6effe3..42dd9b6 100644 --- a/samples/calib/calibrate_camera_charuco.cpp +++ b/samples/calib/calibrate_camera_charuco.cpp @@ -119,19 +119,16 @@ int main(int argc, char *argv[]) { } VideoCapture inputVideo; - - int waitTime; if(!video.empty()) { inputVideo.open(video); waitTime = 0; } else { - inputVideo.open(camId); + char pipe[512]; + sprintf(pipe, "rtsp://192.168.2.64:554/H264?W=1280&H=720&FPS=30&BR=4000000"); + inputVideo.open(pipe); waitTime = 10; } - - - aruco::Dictionary dictionary = aruco::getPredefinedDictionary(0); if (parser.has("d")) { @@ -164,8 +161,6 @@ int main(int argc, char *argv[]) { while(inputVideo.grab()) { Mat image, imageCopy; inputVideo.retrieve(image); - cv::resize(image, image, cv::Size(640, 480)); - vector< int > ids; vector< vector< Point2f > > corners, rejected; diff --git a/samples/demo/aruco_detection.cpp b/samples/demo/aruco_detection.cpp index 83fbabb..9a5ba00 100644 --- a/samples/demo/aruco_detection.cpp +++ b/samples/demo/aruco_detection.cpp @@ -13,9 +13,9 @@ int main(int argc, char *argv[]) { // 打开摄像头 sv::Camera cap; - // cap.setWH(640, 480); - // cap.setFps(30); - cap.open(sv::CameraType::WEBCAM, 2); // CameraID 0 + cap.setWH(1280, 720); + cap.setFps(30); + cap.open(sv::CameraType::WEBCAM, 4); // CameraID 0 // 实例化OpenCV的Mat类，用于内存单帧图像 cv::Mat img; int frame_id = 0; diff --git a/samples/demo/aruco_detection_with_single_object_tracking.cpp b/samples/demo/aruco_detection_with_single_object_tracking.cpp deleted file mode 100644 index 356d55b..0000000 --- a/samples/demo/aruco_detection_with_single_object_tracking.cpp +++ /dev/null @@ -1,173 +0,0 @@ - -#include -#include -// 包含SpireCV SDK头文件 -#include - -using namespace std; - -// 定义窗口名称 -static const std::string RGB_WINDOW = "Image window"; -// 框选到的矩形 -cv::Rect rect_sel; -// 框选起始点 -cv::Point pt_origin; -// 是否按下左键 -bool b_clicked = false; -// 是否得到一个新的框选区域 -bool b_renew_ROI = false; -// 是否开始跟踪 -bool b_begin_TRACK = false; -// 实现框选逻辑的回调函数 -void onMouse(int event, int x, int y, int, void*); - -int main(int argc, char *argv[]) { - // 定义一个新的窗口，可在上面进行框选操作 - cv::namedWindow(RGB_WINDOW); - // 设置窗口操作回调函数，该函数实现整个框选逻辑 - cv::setMouseCallback(RGB_WINDOW, onMouse, 0); - // 实例化 框选目标跟踪类 - sv::SingleObjectTracker sot; - // 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件 - sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); - // sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1280x720.yaml"); - // sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1920x1080.yaml"); - - // 实例化Aruco检测器类 - sv::ArucoDetector ad; - // 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件 - ad.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); - - // 打开摄像头 - sv::Camera cap; - cap.setWH(640, 480); - cap.setFps(30); - cap.open(sv::CameraType::WEBCAM, 2); // CameraID 0 - // cv::VideoCapture cap("/home/amov/SpireCV/test/tracking_1280x720.mp4"); - // 实例化OpenCV的Mat类，用于内存单帧图像 - cv::Mat img; - int frame_id = 0; - while (1) - { - // 实例化SpireCV的 单帧检测结果 接口类 TargetsInFrame - sv::TargetsInFrame tgts(frame_id++); - // 读取一帧图像到img - cap.read(img); - cv::resize(img, img, cv::Size(sot.image_width, sot.image_height)); - - // 开始 单目标跟踪 逻辑 - // 是否有新的目标被手动框选 - if (b_renew_ROI) - { - // 拿新的框选区域 来 初始化跟踪器 - sot.init(img, rect_sel); - // std::cout << rect_sel << std::endl; - // 重置框选标志 - b_renew_ROI = false; - // 开始跟踪 - b_begin_TRACK = true; - } - else if (b_begin_TRACK) - { - // 以前一帧的结果继续跟踪 - sot.track(img, tgts); - - // 执行Aruco二维码检测 - ad.detect(img, tgts); - - // 可视化检测结果，叠加到img上 - sv::drawTargetsInFrame(img, tgts); - - // 控制台打印 单目标跟踪 结果 - printf("Frame-[%d]\n", frame_id); - // 打印当前检测的FPS - printf(" FPS = %.2f\n", tgts.fps); - // 打印当前相机的视场角（degree） - printf(" FOV (fx, fy) = (%.2f, %.2f)\n", tgts.fov_x, tgts.fov_y); - // 打印当前输入图像的像素宽度和高度 - printf(" Frame Size (width, height) = (%d, %d)\n", tgts.width, tgts.height); - if (tgts.targets.size() > 0) - { - printf("Frame-[%d]\n", frame_id); - // 打印 跟踪目标 的中心位置，cx，cy的值域为[0, 1]，以及cx，cy的像素值 - printf(" Tracking Center (cx, cy) = (%.3f, %.3f), in Pixels = ((%d, %d))\n", - tgts.targets[0].cx, tgts.targets[0].cy, - int(tgts.targets[0].cx * tgts.width), - int(tgts.targets[0].cy * tgts.height)); - // 打印 跟踪目标 的外接矩形框的宽度、高度，w，h的值域为(0, 1]，以及w，h的像素值 - printf(" Tracking Size (w, h) = (%.3f, %.3f), in Pixels = ((%d, %d))\n", - tgts.targets[0].w, tgts.targets[0].h, - int(tgts.targets[0].w * tgts.width), - int(tgts.targets[0].h * tgts.height)); - // 打印 跟踪目标 的视线角，跟相机视场相关 - printf(" Tracking Line-of-sight (ax, ay) = (%.3f, %.3f)\n", tgts.targets[0].los_ax, tgts.targets[0].los_ay); - - for (int i=0; i -#include -// 包含SpireCV SDK头文件 -#include - -using namespace std; - -// 定义窗口名称 -static const std::string RGB_WINDOW = "Image window"; -// 框选到的矩形 -cv::Rect rect_sel; -// 框选起始点 -cv::Point pt_origin; - -// 是否得到一个新的框选区域 -bool b_renew_ROI = false; -// 是否开始跟踪 -bool b_begin_TRACK = false; -// 实现框选逻辑的回调函数 -void onMouse(int event, int x, int y, int, void*); - - -struct node { - double x,y; -}; -node p1,p2,p3,p4; -node p; -double getCross(node p1, node p2, node p) { - return (p2.x-p1.x)*(p.y-p1.y)-(p.x-p1.x)*(p2.y-p1.y); -} -bool b_clicked =false; -bool detect_tracking =true; - - -int main(int argc, char *argv[]) { - // 定义一个新的窗口，可在上面进行框选操作 - cv::namedWindow(RGB_WINDOW); - // 设置窗口操作回调函数，该函数实现整个框选逻辑 - cv::setMouseCallback(RGB_WINDOW, onMouse, 0); - // 实例化 框选目标跟踪类 - sv::SingleObjectTracker sot; - // 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件 - sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); - - - sv::CommonObjectDetector cod; - cod.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); - - - // 打开摄像头 - sv::Camera cap; - cap.setWH(640, 480); - cap.setFps(30); - cap.open(sv::CameraType::WEBCAM, 2); // CameraID 0 - // cv::VideoCapture cap("/home/amov/SpireCV/test/tracking_1280x720.mp4"); - // 实例化OpenCV的Mat类，用于内存单帧图像 - cv::Mat img; - int frame_id = 0; - while (1) - { - if (detect_tracking == true) { - // 实例化SpireCV的 单帧检测结果 接口类 TargetsInFrame - sv::TargetsInFrame tgts(frame_id++); - // 读取一帧图像到img - cap.read(img); - cv::resize(img, img, cv::Size(cod.image_width, cod.image_height)); - - // 执行通用目标检测 - cod.detect(img, tgts); - - // 可视化检测结果，叠加到img上 - sv::drawTargetsInFrame(img, tgts); - - // 控制台打印通用目标检测结果 - printf("Frame-[%d]\n", frame_id); - // 打印当前检测的FPS - printf(" FPS = %.2f\n", tgts.fps); - // 打印当前相机的视场角（degree） - printf(" FOV (fx, fy) = (%.2f, %.2f)\n", tgts.fov_x, tgts.fov_y); - for (int i=0; i= 0 && getCross(p2, p3, p) * getCross(p4, p1, p) >= 0) { - b_begin_TRACK = false; - detect_tracking = false; - // pt_origin = cv::Point(nor_x, nor_p_y); - // std::cout << "pt_origin " < 0) - { - printf("Frame-[%d]\n", frame_id); - // 打印 跟踪目标 的中心位置，cx，cy的值域为[0, 1] - printf(" Tracking Center (cx, cy) = (%.3f, %.3f)\n", tgts.targets[0].cx, tgts.targets[0].cy); - // 打印 跟踪目标 的外接矩形框的宽度、高度，w，h的值域为(0, 1] - printf(" Tracking Size (w, h) = (%.3f, %.3f)\n", tgts.targets[0].w, tgts.targets[0].h); - // 打印 跟踪目标 的视线角，跟相机视场相关 - printf(" Tracking Line-of-sight (ax, ay) = (%.3f, %.3f)\n", tgts.targets[0].los_ax, tgts.targets[0].los_ay); - } - } - }//end of tracking - // 显示检测结果img - cv::imshow(RGB_WINDOW, img); - cv::waitKey(1); - } - return 0; -} - -void onMouse(int event, int x, int y, int, void*) -{ - if (b_clicked) - { - // 更新框选区域坐标 - pt_origin.x = 0; - pt_origin.y = 0; - } - // 左键按下 - if (event == cv::EVENT_LBUTTONDOWN) - { - detect_tracking = true; - pt_origin = cv::Point(x, y); - } - - else if (event == cv::EVENT_RBUTTONDOWN) - { - detect_tracking = true; - b_renew_ROI = false; - b_begin_TRACK = false; - b_clicked = true; - } -} diff --git a/samples/demo/common_object_detection.cpp b/samples/demo/common_object_detection.cpp index 544cad2..f051756 100644 --- a/samples/demo/common_object_detection.cpp +++ b/samples/demo/common_object_detection.cpp @@ -13,9 +13,9 @@ int main(int argc, char *argv[]) { // 打开摄像头 sv::Camera cap; - cap.setWH(640, 480); - cap.setFps(60); - cap.open(sv::CameraType::WEBCAM, 2); // CameraID 0 + // cap.setWH(640, 480); + // cap.setFps(30); + cap.open(sv::CameraType::WEBCAM, 0); // CameraID 0 // 实例化OpenCV的Mat类，用于内存单帧图像 cv::Mat img; int frame_id = 0; @@ -65,7 +65,7 @@ int main(int argc, char *argv[]) { // 显示检测结果img cv::imshow("img", img); - cv::waitKey(1); + cv::waitKey(10); } return 0; diff --git a/samples/demo/detection_with_clicked_tracking.cpp b/samples/demo/detection_with_clicked_tracking.cpp index e9c9285..bf03505 100644 --- a/samples/demo/detection_with_clicked_tracking.cpp +++ b/samples/demo/detection_with_clicked_tracking.cpp @@ -40,18 +40,18 @@ int main(int argc, char *argv[]) { // 实例化 框选目标跟踪类 sv::SingleObjectTracker sot; // 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件 - sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); + sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1280x720.yaml"); sv::CommonObjectDetector cod; - cod.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); + cod.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1280x720.yaml"); // 打开摄像头 sv::Camera cap; - // cap.setWH(640, 480); - // cap.setFps(30); - cap.open(sv::CameraType::WEBCAM, 2); // CameraID 0 + cap.setWH(1280, 720); + cap.setFps(30); + cap.open(sv::CameraType::WEBCAM, 4); // CameraID 0 // cv::VideoCapture cap("/home/amov/SpireCV/test/tracking_1280x720.mp4"); // 实例化OpenCV的Mat类，用于内存单帧图像 cv::Mat img; diff --git a/samples/demo/ellipse_detection.cpp b/samples/demo/ellipse_detection.cpp index d427c47..ccfa02b 100644 --- a/samples/demo/ellipse_detection.cpp +++ b/samples/demo/ellipse_detection.cpp @@ -9,13 +9,13 @@ int main(int argc, char *argv[]) { // 实例化 椭圆 检测器类 sv::EllipseDetector ed; // 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件 - ed.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); + ed.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1280x720.yaml"); // 打开摄像头 sv::Camera cap; - cap.setWH(640, 480); + cap.setWH(1280, 720); cap.setFps(30); - cap.open(sv::CameraType::WEBCAM, 0); // CameraID 0 + cap.open(sv::CameraType::WEBCAM, 4); // CameraID 0 // 实例化OpenCV的Mat类，用于内存单帧图像 cv::Mat img; int frame_id = 0; @@ -63,7 +63,7 @@ int main(int argc, char *argv[]) { // 显示检测结果img cv::imshow("img", img); - cv::waitKey(1); + cv::waitKey(10); } return 0; diff --git a/samples/demo/gimbal_detection_with_clicked_tracking.cpp b/samples/demo/gimbal_detection_with_clicked_tracking.cpp index c9d1800..0361076 100644 --- a/samples/demo/gimbal_detection_with_clicked_tracking.cpp +++ b/samples/demo/gimbal_detection_with_clicked_tracking.cpp @@ -66,7 +66,7 @@ int main(int argc, char *argv[]) // 设置获取画面分辨率为720P cap.setWH(1280, 720); // 设置视频帧率为30帧 - cap.setFps(30); + cap.setFps(60); // 开启相机 cap.open(sv::CameraType::G1); @@ -77,10 +77,10 @@ int main(int argc, char *argv[]) // 实例化 框选目标跟踪类 sv::SingleObjectTracker sot; // 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件 - sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); + sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1280x720.yaml"); sv::CommonObjectDetector cod; - cod.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); + cod.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1280x720.yaml"); // 实例化OpenCV的Mat类，用于内存单帧图像 cv::Mat img; @@ -248,4 +248,4 @@ void GimableCallback(double &frame_ang_r, double &frame_ang_p, double &frame_ang count = 0; } count++; -} \ No newline at end of file +} diff --git a/samples/demo/gimbal_landing_marker_detection.cpp b/samples/demo/gimbal_landing_marker_detection.cpp index f6e0357..60e0aad 100644 --- a/samples/demo/gimbal_landing_marker_detection.cpp +++ b/samples/demo/gimbal_landing_marker_detection.cpp @@ -3,7 +3,7 @@ // 包含SpireCV SDK头文件 #include // #include "gimbal_tools.hpp" - +#include using namespace std; // 云台 @@ -43,11 +43,11 @@ int main(int argc, char *argv[]) // 定义相机 sv::Camera cap; // 设置相机流媒体地址为 192.168.2.64 - cap.setIp("192.168.2.64"); + cap.setIp("192.168.1.64"); // 设置获取画面分辨率为720P - cap.setWH(1280, 720); + cap.setWH(640, 480); // 设置视频帧率为30帧 - cap.setFps(30); + cap.setFps(120); // 开启相机 cap.open(sv::CameraType::G1); @@ -58,56 +58,63 @@ int main(int argc, char *argv[]) // 实例化 圆形降落标志 检测器类 sv::LandingMarkerDetector lmd; // 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件 - lmd.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1280x720.yaml"); + lmd.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); // 实例化OpenCV的Mat类，用于内存单帧图像 cv::Mat img; int frame_id = 0; while (1) { + auto time1 = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); // 实例化SpireCV的 单帧检测结果 接口类 TargetsInFrame + for(uint j = 0; j < 60; j++) + { sv::TargetsInFrame tgts(frame_id++); // 读取一帧图像到img cap.read(img); - cv::resize(img, img, cv::Size(lmd.image_width, lmd.image_height)); + //cv::resize(img, img, cv::Size(lmd.image_width, lmd.image_height)); // 执行 降落标志 检测 lmd.detect(img, tgts); // 可视化检测结果，叠加到img上 - sv::drawTargetsInFrame(img, tgts); + //sv::drawTargetsInFrame(img, tgts); // 控制台打印 降落标志 检测结果 - printf("Frame-[%d]\n", frame_id); + //printf("Frame-[%d]\n", frame_id); // 打印当前检测的FPS printf(" FPS = %.2f\n", tgts.fps); // 打印当前相机的视场角（degree） - printf(" FOV (fx, fy) = (%.2f, %.2f)\n", tgts.fov_x, tgts.fov_y); - for (int i = 0; i < tgts.targets.size(); i++) - { + //printf(" FOV (fx, fy) = (%.2f, %.2f)\n", tgts.fov_x, tgts.fov_y); + //for (int i = 0; i < tgts.targets.size(); i++) + //{ // 仅追踪 X 类型的标靶 - if (tgts.targets[i].category_id == 2) - { - gimbalTrack(tgts.targets[0].cx - 0.5f, tgts.targets[0].cy - 0.5f); - } + //if (tgts.targets[i].category_id == 2) + //{ + //gimbalTrack(tgts.targets[0].cx - 0.5f, tgts.targets[0].cy - 0.5f); + //} - printf("Frame-[%d], Marker-[%d]\n", frame_id, i); + //printf("Frame-[%d], Marker-[%d]\n", frame_id, i); // 打印每个 降落标志 的中心位置，cx，cy的值域为[0, 1] - printf(" Marker Center (cx, cy) = (%.3f, %.3f)\n", tgts.targets[i].cx, tgts.targets[i].cy); + //printf(" Marker Center (cx, cy) = (%.3f, %.3f)\n", tgts.targets[i].cx, tgts.targets[i].cy); // 打印每个 降落标志 的外接矩形框的宽度、高度，w，h的值域为(0, 1] - printf(" Marker Size (w, h) = (%.3f, %.3f)\n", tgts.targets[i].w, tgts.targets[i].h); + //printf(" Marker Size (w, h) = (%.3f, %.3f)\n", tgts.targets[i].w, tgts.targets[i].h); // 打印每个 降落标志 的置信度 - printf(" Marker Score = %.3f\n", tgts.targets[i].score); + //printf(" Marker Score = %.3f\n", tgts.targets[i].score); // 打印每个 降落标志 的类别，字符串类型，"h"、"x"、"1"、"2"、"3"... - printf(" Marker Category = %s, Category ID = %d\n", tgts.targets[i].category.c_str(), tgts.targets[i].category_id); + //printf(" Marker Category = %s, Category ID = %d\n", tgts.targets[i].category.c_str(), tgts.targets[i].category_id); // 打印每个 降落标志 的视线角，跟相机视场相关 - printf(" Marker Line-of-sight (ax, ay) = (%.3f, %.3f)\n", tgts.targets[i].los_ax, tgts.targets[i].los_ay); + //printf(" Marker Line-of-sight (ax, ay) = (%.3f, %.3f)\n", tgts.targets[i].los_ax, tgts.targets[i].los_ay); // 打印每个 降落标志 的3D位置（在相机坐标系下），跟降落标志实际半径、相机参数相关 - printf(" Marker Position = (x, y, z) = (%.3f, %.3f, %.3f)\n", tgts.targets[i].px, tgts.targets[i].py, tgts.targets[i].pz); - } + //printf(" Marker Position = (x, y, z) = (%.3f, %.3f, %.3f)\n", tgts.targets[i].px, tgts.targets[i].py, //tgts.targets[i].pz); + //} // 显示检测结果img cv::imshow(RGB_WINDOW, img); - cv::waitKey(10); + cv::waitKey(1); + } + auto time2 = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + auto Ts = time2 - time1; + std::cout << "Ts = " << Ts / (1000) << "us" << std::endl; } return 0; @@ -144,4 +151,4 @@ void GimableCallback(double &frame_ang_r, double &frame_ang_p, double &frame_ang count = 0; } count++; -} \ No newline at end of file +} diff --git a/samples/demo/gimbal_udp_detection_info_sender.cpp b/samples/demo/gimbal_udp_detection_info_sender.cpp index 6dc5fb9..4c5091e 100644 --- a/samples/demo/gimbal_udp_detection_info_sender.cpp +++ b/samples/demo/gimbal_udp_detection_info_sender.cpp @@ -67,7 +67,7 @@ int main(int argc, char *argv[]) // 实例化Aruco检测器类 sv::ArucoDetector ad; // 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件 - ad.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); + ad.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1280x720.yaml"); sv::UDPServer udp; // 实例化OpenCV的Mat类，用于内存单帧图像 @@ -79,6 +79,7 @@ int main(int argc, char *argv[]) sv::TargetsInFrame tgts(frame_id++); // 读取一帧图像到img cap.read(img); + continue; // 执行Aruco二维码检测 ad.detect(img, tgts); diff --git a/samples/demo/landing_marker_detection.cpp b/samples/demo/landing_marker_detection.cpp index ab36c2d..281c73b 100644 --- a/samples/demo/landing_marker_detection.cpp +++ b/samples/demo/landing_marker_detection.cpp @@ -2,7 +2,7 @@ #include // 包含SpireCV SDK头文件 #include - +#include using namespace std; int main(int argc, char *argv[]) { @@ -13,14 +13,18 @@ int main(int argc, char *argv[]) { // 打开摄像头 sv::Camera cap; - // cap.setWH(640, 480); - // cap.setFps(30); - cap.open(sv::CameraType::WEBCAM, 0); // CameraID 0 + cap.setWH(640, 480); + cap.setFps(60); + cap.open(sv::CameraType::WEBCAM, 4); // CameraID 0 // 实例化OpenCV的Mat类，用于内存单帧图像 cv::Mat img; int frame_id = 0; while (1) { + auto time1 = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + // 实例化SpireCV的 单帧检测结果 接口类 TargetsInFrame + for(uint j = 0; j < 60; j++) + { // 实例化SpireCV的 单帧检测结果 接口类 TargetsInFrame sv::TargetsInFrame tgts(frame_id++); // 读取一帧图像到img @@ -33,39 +37,43 @@ int main(int argc, char *argv[]) { sv::drawTargetsInFrame(img, tgts); // 控制台打印 降落标志 检测结果 - printf("Frame-[%d]\n", frame_id); + //printf("Frame-[%d]\n", frame_id); // 打印当前检测的FPS printf(" FPS = %.2f\n", tgts.fps); // 打印当前相机的视场角（degree） - printf(" FOV (fx, fy) = (%.2f, %.2f)\n", tgts.fov_x, tgts.fov_y); + //printf(" FOV (fx, fy) = (%.2f, %.2f)\n", tgts.fov_x, tgts.fov_y); // 打印当前输入图像的像素宽度和高度 - printf(" Frame Size (width, height) = (%d, %d)\n", tgts.width, tgts.height); + //printf(" Frame Size (width, height) = (%d, %d)\n", tgts.width, tgts.height); for (int i=0; i(std::chrono::system_clock::now().time_since_epoch()).count(); + auto Ts = time2 - time1; + std::cout << "Ts = " << Ts / (1000) << "us" << std::endl; } return 0; diff --git a/samples/demo/single_object_tracking.cpp b/samples/demo/single_object_tracking.cpp index 618604b..37eebc9 100644 --- a/samples/demo/single_object_tracking.cpp +++ b/samples/demo/single_object_tracking.cpp @@ -28,15 +28,16 @@ int main(int argc, char *argv[]) { // 实例化 框选目标跟踪类 sv::SingleObjectTracker sot; // 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件 - sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); - // sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1280x720.yaml"); + //sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); + sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1280x720.yaml"); // sot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_1920x1080.yaml"); // 打开摄像头 sv::Camera cap; - // cap.setWH(640, 480); - // cap.setFps(30); - cap.open(sv::CameraType::WEBCAM, 0); // CameraID 0 + cap.setWH(1280, 720); + cap.setFps(30); + cap.open(sv::CameraType::WEBCAM, 4); // CameraID 0 + // cv::VideoCapture cap("/home/amov/SpireCV/test/tracking_1280x720.mp4"); // 实例化OpenCV的Mat类，用于内存单帧图像 cv::Mat img; @@ -92,8 +93,6 @@ int main(int argc, char *argv[]) { int(tgts.targets[0].h * tgts.height)); // 打印 跟踪目标 的视线角，跟相机视场相关 printf(" Tracking Line-of-sight (ax, ay) = (%.3f, %.3f)\n", tgts.targets[0].los_ax, tgts.targets[0].los_ay); - // 打印每个二维码的3D位置（在相机坐标系下），跟二维码实际边长、相机参数相关 - printf(" Tracking Position = (x, y, z) = (%.3f, %.3f, %.3f)\n", tgts.targets[0].px, tgts.targets[0].py, tgts.targets[0].pz); } } diff --git a/samples/demo/veri.cpp b/samples/demo/veri.cpp new file mode 100644 index 0000000..cbe71b6 --- /dev/null +++ b/samples/demo/veri.cpp @@ -0,0 +1,45 @@ +#include +#include +// 包含SpireCV SDK头文件 +#include + +using namespace std; + +int main(int argc, char *argv[]) { + // 打开摄像头 + sv::VeriDetector veri; + veri.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml"); + + cv::VideoCapture cap1("/home/amov/Videos/com/FlyVideo_2023-09-02_11-36-00.avi"); + cv::VideoCapture cap2("/home/amov/Videos/com/FlyVideo_2023-09-02_11-41-55.avi"); + // cap.setWH(640, 480); + // cap.setFps(30); + //cap.open(sv::CameraType::WEBCAM, 0); // CameraID 0 + // 实例化OpenCV的Mat类，用于内存单帧图像 + + + cv::Mat img1,img2; + int frame_id = 0; + while (1) + { + // 实例化SpireCV的 单帧检测结果 接口类 TargetsInFrame + sv::TargetsInFrame tgts(frame_id++); + + // 读取一帧图像到img + cap1.read(img1); + cap2.read(img2); + + cv::resize(img1, img1, cv::Size(224, 224)); + cv::resize(img2, img2, cv::Size(224, 224)); + + veri.detect(img1, img2, tgts); + + + + // 显示img + // cv::imshow("img", img); + // cv::waitKey(10); + } + + return 0; +} diff --git a/samples/test/camera_fps_test.cpp b/samples/test/camera_fps_test.cpp new file mode 100644 index 0000000..468418f --- /dev/null +++ b/samples/test/camera_fps_test.cpp @@ -0,0 +1,34 @@ + +#include +#include +#include +#include + +int main(int argc, char *argv[]) +{ + sv::Camera cap; + cap.setIp(argv[1]); + cap.setWH(640, 480); + cap.setFps(60); + cap.open(sv::CameraType::G1); + + //cap.open(sv::CameraType::WEBCAM, 4); + + cv::Mat img; + //auto time1,time2; + + while (1) + { + auto time1 = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + //for (uint16_t i = 0; i < 120; i++) + //{ + cap.read(img); + cv::imshow("TEST",img); + cv::waitKey(1); + //} + auto time2 = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + auto Ts = time2 - time1; + std::cout << "read 120 count;Ts = " << Ts / (1000) << "us" << std::endl; + std::cout << "average FPS = " << (1000 * 1000 * 1000) / (Ts / 120) << std::endl; + } +} diff --git a/samples/test/gimbal_test.cpp b/samples/test/gimbal_test.cpp new file mode 100644 index 0000000..0db3de6 --- /dev/null +++ b/samples/test/gimbal_test.cpp @@ -0,0 +1,178 @@ +#include +#include +// 包含SpireCV SDK头文件 +#include +#include + +// yaw roll pitch +double gimbalEulerAngle[3]; +bool revFlag = false; +void gimableCallback(double &frame_ang_r, double &frame_ang_p, double &frame_ang_y, + double &imu_ang_r, double &imu_ang_p, double &imu_ang_y, + double &fov_x, double &fov_y) +{ + revFlag = true; + gimbalEulerAngle[0] = imu_ang_r; + gimbalEulerAngle[1] = imu_ang_p; + gimbalEulerAngle[2] = frame_ang_y; +} + +int main(int argc, char *argv[]) +{ + std::cout << "start " << argv[0] << " ...." << std::endl; + + if (argc != 3) + { + std::cout << "param error" << std::endl; + return -1; + } + + sv::Gimbal gimbal(sv::GimbalType::G1, sv::GimbalLink::SERIAL); + + gimbal.setSerialPort(argv[1]); + if (!gimbal.open(gimableCallback)) + { + std::cout << "IO open error" << std::endl; + return -1; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + if (!revFlag) + { + std::cout << "IO error,without data.failed !!!!!" << std::endl; + return -1; + } + + std::cout << " start set home test " << std::endl; + gimbal.setHome(); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + for (uint8_t i = 0; i < 2; i++) + { + if (fabs(gimbalEulerAngle[i]) > 0.1f) + { + std::cout << " gimbal set home error , failed !!!!!" << std::endl; + std::cout << "YRP:" << gimbalEulerAngle[0] << std::endl + << gimbalEulerAngle[1] << std::endl + << gimbalEulerAngle[2] << std::endl; + return -1; + } + } + if (fabs(gimbalEulerAngle[2]) > 3.0f) + { + std::cout << " gimbal set angle error , failed !!!!!" << std::endl; + std::cout << "YRP:" << gimbalEulerAngle[0] << std::endl + << gimbalEulerAngle[1] << std::endl + << gimbalEulerAngle[2] << std::endl; + return -1; + } + + std::cout << " pass... " << std::endl; + std::cout << " start set angle 1 test " << std::endl; + + double angleSet[3] = {30, 90, 45}; + gimbal.setAngleEuler(angleSet[0], angleSet[1], angleSet[2], 0, 0, 0); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + for (uint8_t i = 0; i < 3; i++) + { + if (fabs(gimbalEulerAngle[i] - angleSet[i]) > 1.0f) + { + std::cout << " gimbal set angle error , failed !!!!!" << std::endl; + std::cout << "YRP:" << gimbalEulerAngle[0] << std::endl + << gimbalEulerAngle[1] << std::endl + << gimbalEulerAngle[2] << std::endl; + return -1; + } + } + + std::cout << " pass... " << std::endl; + std::cout << " start set angle 2 test " << std::endl; + + angleSet[0] = -angleSet[0]; + angleSet[1] = -30; + angleSet[2] = -angleSet[2]; + gimbal.setAngleEuler(angleSet[0], angleSet[1], angleSet[2], 0, 0, 0); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + for (uint8_t i = 0; i < 3; i++) + { + if (fabs(gimbalEulerAngle[i] - angleSet[i]) > 1.0f) + { + std::cout << " gimbal set angle error , failed !!!!!" << std::endl; + std::cout << "YRP:" << gimbalEulerAngle[0] << std::endl + << gimbalEulerAngle[1] << std::endl + << gimbalEulerAngle[2] << std::endl; + return -1; + } + } + + std::cout << " pass... " << std::endl; + std::cout << " start set angle rate test " << std::endl; + + gimbal.setHome(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + angleSet[0] = 20; + angleSet[1] = 20; + angleSet[2] = 20; + + for (uint8_t i = 0; i < 51; i++) + { + gimbal.setAngleRateEuler(angleSet[0], angleSet[1], angleSet[2]); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + + gimbal.setAngleRateEuler(0, 0, 0); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + for (uint8_t i = 0; i < 3; i++) + { + if (fabs(gimbalEulerAngle[i] - angleSet[i]) > 0.7f) + { + std::cout << " gimbal set angle rate error , failed !!!!!" << std::endl; + std::cout << "YRP:" << gimbalEulerAngle[0] << std::endl + << gimbalEulerAngle[1] << std::endl + << gimbalEulerAngle[2] << std::endl; + return -1; + } + } + + gimbal.setHome(); + std::cout << " pass... " << std::endl; + std::cout << " start image test " << std::endl; + + sv::Camera cap; + cap.setIp(argv[2]); + cap.setWH(1280, 720); + cap.setFps(30); + + cap.open(sv::CameraType::G1); + + if (!cap.isRunning()) + { + std::cout << " gimbal image error , failed !!!!!" << std::endl; + return -1; + } + + cv::Mat img; + uint16_t count = 0; + + for (uint16_t i = 0; i < 300; i++) + { + if (cap.read(img)) + { + count++; + if (count > 10) + { + std::cout << " pass... " << std::endl; + return 0; + } + } + + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + std::cout << " gimbal image error , failed !!!!!" << std::endl; + return -1; +} \ No newline at end of file diff --git a/scripts/test/test_gimbal.sh b/scripts/test/test_gimbal.sh new file mode 100755 index 0000000..315f965 --- /dev/null +++ b/scripts/test/test_gimbal.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e + +./GimbalTest /dev/ttyUSB0 192.168.2.64 + diff --git a/sv_algorithm_params.json b/sv_algorithm_params.json index 2d4edb1..47ac746 100644 --- a/sv_algorithm_params.json +++ b/sv_algorithm_params.json @@ -1,23 +1,11 @@ { "CommonObjectDetector": { -<<<<<<< Updated upstream - "dataset": "COCO", - "inputSize": 1280, - "nmsThrs": 0.6, - "scoreThrs": 0.3, - "useWidthOrHeight": 1, - "withSegmentation": false, -======= "dataset": "CAR", "inputSize": 640, "nmsThrs": 0.6, - "scoreThrs": 0.4, + "scoreThrs": 0.6, "useWidthOrHeight": 1, "withSegmentation": false, - "datasetCAR":{ - "diff_car":[0.12,0.105] - }, ->>>>>>> Stashed changes "datasetPersonVehicle": { "person": [0.5, 1.8], "car": [4.1, 1.5], @@ -31,6 +19,9 @@ "datasetDrone": { "drone": [0.4, 0.2] }, + "datasetCAR": { + "car": [0.12, 0.1] + }, "datasetCOCO": { "person": [-1, -1], "bicycle": [-1, -1], @@ -126,28 +117,27 @@ "SingleObjectTracker": { "algorithm": "nano", "backend": 0, -<<<<<<< Updated upstream "target": 0, "useWidthOrHeight": 0, - "sigleobjectW":2, + "sigleobjectW":0.126, "sigleobjectH":-1 }, - "LandingMarkerDetector": { - "labels": ["h","x","1","2","8"], - "maxCandidates": 3 - }, - "EllipseDetector": { - "radiusInMeter": 0.1535, -======= - "target": 0 + "MultipleObjectTracker": { + "algorithm": "sort", + "with_reid": false, + "reid_input_size": [128, 128], + "reid_num_samples": 10, + "reid_match_thres": 2.0, + "iou_thres": 0.5, + "max_age": 10, + "min_hits": 3 }, "LandingMarkerDetector": { "labels": ["h"], "maxCandidates": 5 }, "EllipseDetector": { - "radiusInMeter": 0.685, ->>>>>>> Stashed changes + "radiusInMeter": 0.5, "preProcessingGaussKernel": 5, "preProcessingGaussSigma": 1.306, "thPosition": 1.0, @@ -155,17 +145,10 @@ "minEdgeLength": 9, "minOrientedRectSide": 2.984, "distanceToEllipseContour": 0.111, -<<<<<<< Updated upstream - "minScore": 0.7, - "minReliability": 0.470, - "ns": 22, - "percentNe": 0.99, -======= "minScore": 0.511, "minReliability": 0.470, "ns": 22, "percentNe": 0.946, ->>>>>>> Stashed changes "T_CNC": 0.121, "T_TCN_L": 0.468, "T_TCN_P": 0.560, @@ -174,13 +157,8 @@ "ArucoDetector": { "dictionaryId": 10, "markerIds": [-1], -<<<<<<< Updated upstream - "markerLengths": [0.2], - "adaptiveThreshConstant": 35, -======= "markerLengths": [0.17], "adaptiveThreshConstant": 7, ->>>>>>> Stashed changes "adaptiveThreshWinSizeMax": 23, "adaptiveThreshWinSizeMin": 3, "adaptiveThreshWinSizeStep": 10, diff --git a/sv_algorithm_params_coco_1280.json b/sv_algorithm_params_coco_1280.json deleted file mode 100644 index 13dc7d1..0000000 --- a/sv_algorithm_params_coco_1280.json +++ /dev/null @@ -1,184 +0,0 @@ -{ - "CommonObjectDetector": { - "dataset": "COCO", - "inputSize": 1280, - "nmsThrs": 0.6, - "scoreThrs": 0.4, - "useWidthOrHeight": 1, - "withSegmentation": false, - "datasetPersonVehicle": { - "person": [0.5, 1.8], - "car": [4.1, 1.5], - "bus": [10, 3], - "truck": [-1, -1], - "bike": [-1, -1], - "train": [-1, -1], - "boat": [-1, -1], - "aeroplane": [-1, -1] - }, - "datasetDrone": { - "drone": [0.4, 0.2] - }, - "datasetCOCO": { - "person": [-1, -1], - "bicycle": [-1, -1], - "car": [-1, -1], - "motorcycle": [-1, -1], - "airplane": [-1, -1], - "bus": [-1, -1], - "train": [-1, -1], - "truck": [-1, -1], - "boat": [-1, -1], - "traffic light": [-1, -1], - "fire hydrant": [-1, -1], - "stop sign": [-1, -1], - "parking meter": [-1, -1], - "bench": [-1, -1], - "bird": [-1, -1], - "cat": [-1, -1], - "dog": [-1, -1], - "horse": [-1, -1], - "sheep": [-1, -1], - "cow": [-1, -1], - "elephant": [-1, -1], - "bear": [-1, -1], - "zebra": [-1, -1], - "giraffe": [-1, -1], - "backpack": [-1, -1], - "umbrella": [-1, -1], - "handbag": [-1, -1], - "tie": [-1, -1], - "suitcase": [-1, -1], - "frisbee": [-1, -1], - "skis": [-1, -1], - "snowboard": [-1, -1], - "sports ball": [-1, -1], - "kite": [-1, -1], - "baseball bat": [-1, -1], - "baseball glove": [-1, -1], - "skateboard": [-1, -1], - "surfboard": [-1, -1], - "tennis racket": [-1, -1], - "bottle": [-1, -1], - "wine glass": [-1, -1], - "cup": [-1, -1], - "fork": [-1, -1], - "knife": [-1, -1], - "spoon": [-1, -1], - "bowl": [-1, -1], - "banana": [-1, -1], - "apple": [-1, -1], - "sandwich": [-1, -1], - "orange": [-1, -1], - "broccoli": [-1, -1], - "carrot": [-1, -1], - "hot dog": [-1, -1], - "pizza": [-1, -1], - "donut": [-1, -1], - "cake": [-1, -1], - "chair": [-1, -1], - "couch": [-1, -1], - "potted plant": [-1, -1], - "bed": [-1, -1], - "dining table": [-1, -1], - "toilet": [-1, -1], - "tv": [-1, -1], - "laptop": [-1, -1], - "mouse": [-1, -1], - "remote": [-1, -1], - "keyboard": [-1, -1], - "cell phone": [-1, -1], - "microwave": [-1, -1], - "oven": [-1, -1], - "toaster": [-1, -1], - "sink": [-1, -1], - "refrigerator": [-1, -1], - "book": [-1, -1], - "clock": [-1, -1], - "vase": [-1, -1], - "scissors": [-1, -1], - "teddy bear": [-1, -1], - "hair drier": [-1, -1], - "toothbrush": [-1, -1] - } - }, - "AutoFocusObjectDetector": { - "lock_thres": 5, - "unlock_thres": 5, - "lock_scale_init": 12.0, - "lock_scale": 8.0, - "categories_filter": [], - "keep_unlocked": false, - "use_square_region": false - }, - "SingleObjectTracker": { - "algorithm": "siamrpn", - "backend": 0, - "target": 0 - }, - "LandingMarkerDetector": { - "labels": ["x", "h"], - "maxCandidates": 5 - }, - "EllipseDetector": { - "radiusInMeter": 0.5, - "preProcessingGaussKernel": 5, - "preProcessingGaussSigma": 1.306, - "thPosition": 1.0, - "maxCenterDistance": 0.05, - "minEdgeLength": 9, - "minOrientedRectSide": 2.984, - "distanceToEllipseContour": 0.111, - "minScore": 0.511, - "minReliability": 0.470, - "ns": 22, - "percentNe": 0.946, - "T_CNC": 0.121, - "T_TCN_L": 0.468, - "T_TCN_P": 0.560, - "thRadius": 0.202 - }, - "ArucoDetector": { - "dictionaryId": 10, - "markerIds": [-1], - "markerLengths": [0.2], - "adaptiveThreshConstant": 7, - "adaptiveThreshWinSizeMax": 23, - "adaptiveThreshWinSizeMin": 3, - "adaptiveThreshWinSizeStep": 10, - "aprilTagCriticalRad": 0.17453292519, - "aprilTagDeglitch": 0, - "aprilTagMaxLineFitMse": 10.0, - "aprilTagMaxNmaxima": 10, - "aprilTagMinClusterPixels": 5, - "aprilTagMinWhiteBlackDiff": 5, - "aprilTagQuadDecimate": 0.0, - "aprilTagQuadSigma": 0.0, - "cornerRefinementMaxIterations": 30, - "cornerRefinementMethod": 0, - "cornerRefinementMinAccuracy": 0.1, - "cornerRefinementWinSize": 5, - "detectInvertedMarker": false, - "errorCorrectionRate": 0.6, - "markerBorderBits": 1, - "maxErroneousBitsInBorderRate": 0.35, - "maxMarkerPerimeterRate": 4.0, - "minCornerDistanceRate": 0.05, - "minDistanceToBorder": 3, - "minMarkerDistanceRate": 0.05, - "minMarkerLengthRatioOriginalImg": 0, - "minMarkerPerimeterRate": 0.03, - "minOtsuStdDev": 5.0, - "minSideLengthCanonicalImg": 32, - "perspectiveRemoveIgnoredMarginPerCell": 0.13, - "perspectiveRemovePixelPerCell": 4, - "polygonalApproxAccuracyRate": 0.03, - "useAruco3Detection": false - }, - "ColorLineDetector": { - "line_color": "black", - "line_location": 0.5, - "line_location_a1": 0.3, - "line_location_a2": 0.7 - } -} diff --git a/sv_algorithm_params_coco_640.json b/sv_algorithm_params_coco_640.json deleted file mode 100644 index 8ccf8a7..0000000 --- a/sv_algorithm_params_coco_640.json +++ /dev/null @@ -1,184 +0,0 @@ -{ - "CommonObjectDetector": { - "dataset": "COCO", - "inputSize": 640, - "nmsThrs": 0.6, - "scoreThrs": 0.4, - "useWidthOrHeight": 1, - "withSegmentation": true, - "datasetPersonVehicle": { - "person": [0.5, 1.8], - "car": [4.1, 1.5], - "bus": [10, 3], - "truck": [-1, -1], - "bike": [-1, -1], - "train": [-1, -1], - "boat": [-1, -1], - "aeroplane": [-1, -1] - }, - "datasetDrone": { - "drone": [0.4, 0.2] - }, - "datasetCOCO": { - "person": [-1, -1], - "bicycle": [-1, -1], - "car": [-1, -1], - "motorcycle": [-1, -1], - "airplane": [-1, -1], - "bus": [-1, -1], - "train": [-1, -1], - "truck": [-1, -1], - "boat": [-1, -1], - "traffic light": [-1, -1], - "fire hydrant": [-1, -1], - "stop sign": [-1, -1], - "parking meter": [-1, -1], - "bench": [-1, -1], - "bird": [-1, -1], - "cat": [-1, -1], - "dog": [-1, -1], - "horse": [-1, -1], - "sheep": [-1, -1], - "cow": [-1, -1], - "elephant": [-1, -1], - "bear": [-1, -1], - "zebra": [-1, -1], - "giraffe": [-1, -1], - "backpack": [-1, -1], - "umbrella": [-1, -1], - "handbag": [-1, -1], - "tie": [-1, -1], - "suitcase": [-1, -1], - "frisbee": [-1, -1], - "skis": [-1, -1], - "snowboard": [-1, -1], - "sports ball": [-1, -1], - "kite": [-1, -1], - "baseball bat": [-1, -1], - "baseball glove": [-1, -1], - "skateboard": [-1, -1], - "surfboard": [-1, -1], - "tennis racket": [-1, -1], - "bottle": [-1, -1], - "wine glass": [-1, -1], - "cup": [-1, -1], - "fork": [-1, -1], - "knife": [-1, -1], - "spoon": [-1, -1], - "bowl": [-1, -1], - "banana": [-1, -1], - "apple": [-1, -1], - "sandwich": [-1, -1], - "orange": [-1, -1], - "broccoli": [-1, -1], - "carrot": [-1, -1], - "hot dog": [-1, -1], - "pizza": [-1, -1], - "donut": [-1, -1], - "cake": [-1, -1], - "chair": [-1, -1], - "couch": [-1, -1], - "potted plant": [-1, -1], - "bed": [-1, -1], - "dining table": [-1, -1], - "toilet": [-1, -1], - "tv": [-1, -1], - "laptop": [-1, -1], - "mouse": [-1, -1], - "remote": [-1, -1], - "keyboard": [-1, -1], - "cell phone": [-1, -1], - "microwave": [-1, -1], - "oven": [-1, -1], - "toaster": [-1, -1], - "sink": [-1, -1], - "refrigerator": [-1, -1], - "book": [-1, -1], - "clock": [-1, -1], - "vase": [-1, -1], - "scissors": [-1, -1], - "teddy bear": [-1, -1], - "hair drier": [-1, -1], - "toothbrush": [-1, -1] - } - }, - "AutoFocusObjectDetector": { - "lock_thres": 5, - "unlock_thres": 5, - "lock_scale_init": 12.0, - "lock_scale": 8.0, - "categories_filter": [], - "keep_unlocked": false, - "use_square_region": false - }, - "SingleObjectTracker": { - "algorithm": "siamrpn", - "backend": 0, - "target": 0 - }, - "LandingMarkerDetector": { - "labels": ["x", "h"], - "maxCandidates": 5 - }, - "EllipseDetector": { - "radiusInMeter": 0.5, - "preProcessingGaussKernel": 5, - "preProcessingGaussSigma": 1.306, - "thPosition": 1.0, - "maxCenterDistance": 0.05, - "minEdgeLength": 9, - "minOrientedRectSide": 2.984, - "distanceToEllipseContour": 0.111, - "minScore": 0.511, - "minReliability": 0.470, - "ns": 22, - "percentNe": 0.946, - "T_CNC": 0.121, - "T_TCN_L": 0.468, - "T_TCN_P": 0.560, - "thRadius": 0.202 - }, - "ArucoDetector": { - "dictionaryId": 10, - "markerIds": [-1], - "markerLengths": [0.2], - "adaptiveThreshConstant": 7, - "adaptiveThreshWinSizeMax": 23, - "adaptiveThreshWinSizeMin": 3, - "adaptiveThreshWinSizeStep": 10, - "aprilTagCriticalRad": 0.17453292519, - "aprilTagDeglitch": 0, - "aprilTagMaxLineFitMse": 10.0, - "aprilTagMaxNmaxima": 10, - "aprilTagMinClusterPixels": 5, - "aprilTagMinWhiteBlackDiff": 5, - "aprilTagQuadDecimate": 0.0, - "aprilTagQuadSigma": 0.0, - "cornerRefinementMaxIterations": 30, - "cornerRefinementMethod": 0, - "cornerRefinementMinAccuracy": 0.1, - "cornerRefinementWinSize": 5, - "detectInvertedMarker": false, - "errorCorrectionRate": 0.6, - "markerBorderBits": 1, - "maxErroneousBitsInBorderRate": 0.35, - "maxMarkerPerimeterRate": 4.0, - "minCornerDistanceRate": 0.05, - "minDistanceToBorder": 3, - "minMarkerDistanceRate": 0.05, - "minMarkerLengthRatioOriginalImg": 0, - "minMarkerPerimeterRate": 0.03, - "minOtsuStdDev": 5.0, - "minSideLengthCanonicalImg": 32, - "perspectiveRemoveIgnoredMarginPerCell": 0.13, - "perspectiveRemovePixelPerCell": 4, - "polygonalApproxAccuracyRate": 0.03, - "useAruco3Detection": false - }, - "ColorLineDetector": { - "line_color": "black", - "line_location": 0.5, - "line_location_a1": 0.3, - "line_location_a2": 0.7 - } -} diff --git a/video_io/sv_video_base.cpp b/video_io/sv_video_base.cpp index cb57fe8..a0aa288 100644 --- a/video_io/sv_video_base.cpp +++ b/video_io/sv_video_base.cpp @@ -368,6 +368,58 @@ void UDPServer::send(const TargetsInFrame& tgts_) } +void drawTargetsInFrame( + cv::Mat& img_, + const TargetsInFrame& tgts_, + int aruco_track_id, + bool with_all, + bool with_category, + bool with_tid, + bool with_seg, + bool with_box, + bool with_ell, + bool with_aruco, + bool with_yaw +) +{ + if (tgts_.rois.size() > 0 ) + { + cv::Mat image_ret; + cv::addWeighted(img_, 0.5, cv::Mat::zeros(cv::Size(img_.cols, img_.rows), CV_8UC3), 0, 0, image_ret); + cv::Rect roi = cv::Rect(tgts_.rois[0].x1, tgts_.rois[0].y1, tgts_.rois[0].x2 - tgts_.rois[0].x1, tgts_.rois[0].y2 - tgts_.rois[0].y1); + img_(roi).copyTo(image_ret(roi)); + image_ret.copyTo(img_); + } + std::vector > aruco_corners; + std::vector aruco_ids; + for (Target tgt : tgts_.targets) + { + if ((with_all || with_aruco) && tgt.has_aruco && (tgt.tracked_id == aruco_track_id)) + { + std::vector a_corners; + int a_id; + if (tgt.getAruco(a_id, a_corners)) { aruco_ids.push_back(a_id); aruco_corners.push_back(a_corners); } + cv::circle(img_, cv::Point(int(tgt.cx * tgts_.width), int(tgt.cy * tgts_.height)), 4, cv::Scalar(0,255,0), 2); + } + if ((with_all || with_box) && tgt.has_box && (tgt.tracked_id == aruco_track_id)) + { + Box b; + tgt.getBox(b); + cv::rectangle(img_, cv::Rect(b.x1, b.y1, b.x2-b.x1+1, b.y2-b.y1+1), cv::Scalar(0,0,255), 1, 1, 0); + if ((with_all || with_category) && tgt.has_category) + { + cv::putText(img_, tgt.category, cv::Point(b.x1, b.y1-4), cv::FONT_HERSHEY_DUPLEX, 0.4, cv::Scalar(255,0,0)); + } + } + } + if ((with_all || with_aruco) && aruco_ids.size() > 0) + { + cv::aruco::drawDetectedMarkers(img_, aruco_corners, aruco_ids); + } +} + + + void drawTargetsInFrame( cv::Mat& img_, const TargetsInFrame& tgts_, @@ -1103,33 +1155,57 @@ void CameraBase::_run() { while (this->_is_running && this->_cap.isOpened()) { - this->_cap >> this->_frame; - this->_is_updated = true; - std::this_thread::sleep_for(std::chrono::milliseconds(2)); + //this->_cap >> this->_frame; + //this->_is_updated = true; + //std::this_thread::sleep_for(std::chrono::milliseconds(2)); + + if(this->_cap.grab()) + { + std::lock_guard locker(this->_frame_mutex); + this->_cap.retrieve(this->_frame); + this->_frame_empty.notify_all(); + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } } bool CameraBase::read(cv::Mat& image) { + bool ret = false; if (this->_type == CameraType::WEBCAM || this->_type == CameraType::G1 || this->_type == CameraType::MIPI) { - int n_try = 0; - while (n_try < 5000) + std::lock_guard locker(this->_frame_mutex); + if(this->_frame_empty.wait_for(this->_frame_mutex,std::chrono::milliseconds(2000)) == std::cv_status::no_timeout) { - if (this->_is_updated) - { - this->_is_updated = false; - this->_frame.copyTo(image); - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(20)); - n_try ++; + this->_frame.copyTo(image); + ret = true; + } + else + { + throw std::runtime_error("SpireCV (101) Camera cannot OPEN, check CAMERA_ID!"); } } - if (image.cols == 0 || image.rows == 0) - { - throw std::runtime_error("SpireCV (101) Camera cannot OPEN, check CAMERA_ID!"); - } - return image.cols > 0 && image.rows > 0; + return ret; + + if (this->_type == CameraType::WEBCAM || this->_type == CameraType::G1 || this->_type == CameraType::MIPI) + { + int n_try = 0; + while (n_try < 5000) + { + if (this->_is_updated) + { + this->_is_updated = false; + this->_frame.copyTo(image); + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + n_try ++; + } + } + if (image.cols == 0 || image.rows == 0) + { + throw std::runtime_error("SpireCV (101) Camera cannot OPEN, check CAMERA_ID!"); + } + return image.cols > 0 && image.rows > 0; } void CameraBase::release() {