floraachy
/
SpireCV
forked from floratest1/SpireCV
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update p250.

This commit is contained in:
Eason Yi 2024-02-04 18:42:07 +08:00
parent 1a19b6c7f3
commit 1656e806ba
66 changed files with 2840 additions and 2235 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
CMakeLists.txt
View File

@ -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)

290
algorithm/mot/sv_mot.cpp
View File

@ -1,6 +1,8 @@
#include "sv_mot.h"
#include <cmath>
#include <fstream>
#include <limits>
#include <vector>
#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<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4d &bbox)
{
Matrix<double,8,1> mean;
mean << bbox(0), bbox(1), bbox(2) / bbox(3), bbox(3), 0, 0, 0, 0;
Matrix<double,4,1> 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<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::update(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> 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<double, 8, 1> new_mean = mean + Kalman_gain * (measurement - this->_H * mean);
Matrix<double, 8, 8> 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<double, 4, 1>, Matrix<double, 4, 4> > projected = project(mean, covariances);
Matrix<double, 4, 1> projected_mean = projected.first;
Matrix<double, 4, 4> projected_cov = projected.second;
Eigen::LLT<Eigen::MatrixXd> chol_factor(projected_cov);
MatrixXd Kalman_gain = (chol_factor.solve((covariances * this->_H.transpose()).transpose())).transpose();
VectorXd innovation = measurement - projected_mean;
Matrix<double, 8, 1> new_mean = mean + Kalman_gain *innovation;
Matrix<double, 8, 8> new_covariances = covariances - Kalman_gain * projected_cov * Kalman_gain.transpose();
return make_pair(new_mean, new_covariances);
}
pair<Matrix<double, 4, 1>, Matrix<double, 4, 4> > KalmanFilter::project(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> 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<double, 4, 1> pro_mean = this->_H * mean;
Matrix<double, 4, 4> pro_covariances = this->_H * covariances * this->_H.transpose() + R;
return make_pair(pro_mean, pro_covariances);
}
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::predict(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> 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<double, 8, 1> pre_mean = this->_F * mean;
Matrix<double, 8, 8> pre_cov = this->_F * covariances * this->_F.transpose() + Q;
Matrix<double, 8, 8> 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<tgts.targets.size(); i++)
@ -187,34 +203,36 @@ void SORT::update(TargetsInFrame& tgts)
tgts.targets[i].getBox(box);
Tracklet tracklet;
tracklet.id = ++ this->_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<double, 8, 1>, Matrix<double, 8, 8> > 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<tgts.targets.size(); i++)
{
tgts.targets[i].tracked_id = 0;
tgts.targets[i].has_tid = true;
}
array<int, 100> match_det;
match_det.fill(-1);
vector<int> 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<vector<double> > iouMatrix(this->_tracklets.size(), vector<double> (tgts.targets.size(), 0));
for (int i=0; i<this->_tracklets.size(); i++)
{
@ -238,6 +254,7 @@ void SORT::update(TargetsInFrame& tgts)
iouMatrix[i][j] = this->_iou(this->_tracklets[i], box);
}
}
vector<pair<int, int> > 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 <vector<double>> ().swap(iouMatrix);
for (int i=0; i<tgts.targets.size(); i++)
{
// cout << "match_det: index: " << i << " value: " << match_det[i] << endl;
if (match_det[i] == -1)
{
// cout << "create new tracklet." << endl;
sv::Box box;
tgts.targets[i].getBox(box);
Tracklet tracklet;
tracklet.id = ++ this->_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<double, 8, 1>, Matrix<double, 8, 8> > 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<double, 8, 1>, Matrix<double, 8, 8> > 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<Tracklet> 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<double>& vec) {
double minVal = std::numeric_limits<double>::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<std::vector<double>>& 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<std::vector<double>>& costMatrix) {
for (size_t col = 0; col < costMatrix[0].size(); ++col) {
double minVal = std::numeric_limits<double>::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<pair<int, int> > SORT::_hungarian(vector<vector<double> > 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<vector<double> > transposedMatrix(numCols, vector<double>(numRows));
for (int i=0; i<numRows; i++)
if (transposed) {
vector<vector<double>> transposedMatrix(numCols, vector<double>(numRows));
for (int i = 0; i < numRows; i++)
{
for (int j=0; j<numCols; j++)
for (int j = 0; j < numCols; j++)
{
transposedMatrix[j][i] = costMatrix[i][j];
}
@ -359,76 +421,58 @@ vector<pair<int, int> > SORT::_hungarian(vector<vector<double> > costMatrix)
costMatrix = transposedMatrix;
swap(numRows, numCols);
}
vector<double>rowMin(numRows, numeric_limits<double>::infinity());
vector<double>colMin(numCols, numeric_limits<double>::infinity());
vector<int>rowMatch(numRows, -1);
vector<int>colMatch(numCols, -1);
vector<pair<int, int> > matches;
// step1: Subtract the row minimums from each row
for (int i=0; i<numRows; i++)
{
for (int j=0; j<numCols; j++)
{
rowMin[i] = min(rowMin[i], costMatrix[i][j]);
}
for (int j=0; j<numCols; j++)
{
costMatrix[i][j] -= rowMin[i];
}
}
// step2: substract the colcum minimums from each column
for (int j=0; j<numCols; j++)
{
for (int i=0; i<numRows; i++)
{
colMin[j] = min(colMin[j], costMatrix[i][j]);
}
for (int i=0; i<numRows; i++)
{
costMatrix[i][j] -= colMin[j];
}
}
// step3: find a maximal matching
for (int i=0; i<numRows; i++)
{
vector<bool> visited(numCols, false);
this->_augment(costMatrix, i, rowMatch, colMatch, visited);
}
// step4: calculate the matches
matches.clear();
for (int j=0; j<numCols; j++)
{
matches.push_back(make_pair(colMatch[j], j));
}
if (transposed)
{
for (auto& match : matches)
{
swap(match.first, match.second);
}
}
return matches;
}
// Determine the larger dimension for matching
size_t maxDim = std::max(numRows, numCols);
bool SORT::_augment(const vector<vector<double> >& costMatrix, int row, vector<int>& rowMatch, vector<int>& colMatch, vector<bool>& visited)
{
int numCols = costMatrix[0].size();
for (int j=0; j<numCols; j++)
{
if (costMatrix[row][j] == 0 && !visited[j])
{
visited[j] = true;
if (colMatch[j] == -1 || this->_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<std::vector<double>> squareMatrix(maxDim, std::vector<double>(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<int> rowAssignment(maxDim, -1);
std::vector<int> colAssignment(maxDim, -1);
// Perform the matching
for (size_t row = 0; row < maxDim; ++row) {
std::vector<bool> 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<int, int> format
std::vector<std::pair<int, int>> 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;
}
}

4
algorithm/sot/ocv470/sot_ocv470_impl.cpp
View File

@ -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);
}

1
algorithm/sv_algorithm_base.cpp
View File

@ -1248,3 +1248,4 @@ namespace sv
}
}

189
algorithm/veri/cuda/veri_det_cuda_impl.cpp Normal file
View File

@ -0,0 +1,189 @@
#include "veri_det_cuda_impl.h"
#include <cmath>
#include <fstream>
#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 <iostream>
#include <cmath>
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<cv::Mat> &input_rois_,
std::vector<int> &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
}

51
algorithm/veri/cuda/veri_det_cuda_impl.h Normal file
View File

@ -0,0 +1,51 @@
#ifndef __SV_VERI_DET_CUDA__
#define __SV_VERI_DET_CUDA__
#include "sv_core.h"
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/tracking.hpp>
#include <string>
#include <chrono>
#ifdef WITH_CUDA
#include <NvInfer.h>
#include <cuda_runtime_api.h>
#endif
namespace sv {
class VeriDetectorCUDAImpl
{
public:
VeriDetectorCUDAImpl();
~VeriDetectorCUDAImpl();
bool cudaSetup();
void cudaRoiCNN(
std::vector<cv::Mat>& input_rois_,
std::vector<int>& 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

61
algorithm/veri/sv_veri_det.cpp Normal file
View File

@ -0,0 +1,61 @@
#include "sv_veri_det.h"
#include <cmath>
#include <fstream>
#ifdef WITH_CUDA
#include <NvInfer.h>
#include <cuda_runtime_api.h>
#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<cv::Mat>& input_rois_,
std::vector<int>& 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<cv::Mat> e_roi = {img1_, img2_};
std::vector<int> output_labels;
roiCNN(e_roi, output_labels);
}
}

9
calib_webcam_1280x720.yaml
View File

@ -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

20
calib_webcam_1920x1080.yaml Normal file
View File

@ -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

14
calib_webcam_640x480.yaml
View File

@ -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

38
gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h Executable file
View File

@ -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

425
gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp Executable file
View File

@ -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<AT10::GIMBAL_EXTEND_FRAME_T *>(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<AT10::GIMBAL_RCV_POS_MSG_T *>(((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<AT10::GIMBAL_STD_FRAME_T *>(buf);
switch (stdTemp->cmd)
{
case AT10::GIMBAL_CMD_STD_RCV_STATE:
AT10::GIMBAL_RCV_STD_STATE_MSG_T *tempRcv;
tempRcv = reinterpret_cast<AT10::GIMBAL_RCV_STD_STATE_MSG_T *>(((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<AT10::GIMBAL_EXTEND_FRAME_T *>(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<AT10::GIMBAL_STD_FRAME_T *>(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;
}

82
gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h Executable file
View File

@ -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 <mutex>
#include <malloc.h>
#include <iostream>
#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

208
gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp Executable file
View File

@ -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<AT10::AT10_EXT_CMD_T *>(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;

174
gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h Executable file
View File

@ -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 <stdint.h>
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

36
gimbal_ctrl/driver/src/CMakeLists.txt Executable file
View File

@ -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}
)

13
gimbal_ctrl/driver/src/FIFO/CMakeLists.txt Executable file
View File

@ -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}
)

212
gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cc
View File

@ -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 <string.h>
#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;
}

228
gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cpp Executable file
View File

@ -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 <string.h>
#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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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;
}

60
gimbal_ctrl/driver/src/FIFO/Ring_Fifo.h
View File

@ -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 <thread>
#include <mutex>
#include <condition_variable>
#include <iostream>
#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

76
gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp
View File

@ -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
{

46
gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h
View File

@ -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 <mutex>
#include <malloc.h>
@ -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

117
gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp
View File

@ -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 <math.h>
/**
* 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<uint8_t *>(&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<uint8_t *>(&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<G1::GIMBAL_STD_MSG_T *>(cmd);
return pack(tempCmd->cmd, tempCmd->data, tempCmd->len);
}

21
gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h
View File

@ -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()
}

166
gimbal_ctrl/driver/src/G2/g2_gimbal_crc.h
View File

@ -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

243
gimbal_ctrl/driver/src/G2/g2_gimbal_driver.cpp
View File

@ -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<G2::GIMBAL_FRAME_T *>(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;
}

76
gimbal_ctrl/driver/src/G2/g2_gimbal_driver.h
View File

@ -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 <mutex>
#include <malloc.h>
#include <iostream>
#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

81
gimbal_ctrl/driver/src/G2/g2_gimbal_funtion.cpp
View File

@ -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;
}

81
gimbal_ctrl/driver/src/G2/g2_gimbal_struct.h
View File

@ -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 <stdint.h>
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

78
gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp
View File

@ -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
{

27
gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h
View File

@ -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 <mutex>
#include <malloc.h>
@ -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

2
gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp
View File

@ -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"

2
gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h
View File

@ -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

163
gimbal_ctrl/driver/src/amov_gimabl.cpp → gimbal_ctrl/driver/src/amov_gimbal.cpp
View File

@ -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 <iostream>
#include <thread>
@ -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<std::string, createCallback> callbackMap;
std::map<std::string, AMOV_GIMBAL_TYPE_T> 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

65
gimbal_ctrl/driver/src/amov_gimbal.h
View File

@ -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 <stdbool.h>
#include <iostream>
#include <thread>
#include <unistd.h>
#include <mutex>
#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;

79
gimbal_ctrl/driver/src/amov_gimbal_private.h Normal file
View File

@ -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 <stdint.h>
#include <stdbool.h>
#include <iostream>
#include <thread>
#include <unistd.h>
#include <mutex>
#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

25
gimbal_ctrl/driver/src/amov_gimbal_struct.h
View File

@ -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 <stdint.h>
#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

29
gimbal_ctrl/driver/src/amov_tool.h Normal file
View File

@ -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;
}
}

140
gimbal_ctrl/sv_gimbal.cpp
View File

@ -15,6 +15,28 @@
#include <iostream>
#include <string>
#include <thread>
namespace sv
{
std::map<std::string, void *> Gimbal::IOList;
std::mutex Gimbal::IOListMutex;
std::map<GimbalType, std::string> gimbaltypeList =
{
{GimbalType::G1, "G1"},
{GimbalType::Q10f, "Q10f"},
{GimbalType::AT10, "AT10"}};
std::string &cvGimbalType2Str(const GimbalType &type)
{
std::map<GimbalType, std::string>::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<std::string, void *>::iterator list = IOList.find(dev->m_serial_port);
std::pair<std::string, void *> 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;
}

1
include/sv_algorithm_base.h
View File

@ -177,3 +177,4 @@ protected:
}
#endif

11
include/sv_gimbal.h
View File

@ -10,6 +10,10 @@
#define __SV_GIMBAL__
#include <string>
#include <map>
#include <iterator>
#include <thread>
#include <mutex>
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<std::string, void *> IOList;
static std::mutex IOListMutex;
static void *creatIO(Gimbal *dev);
static void removeIO(Gimbal *dev);
public:
//! Constructor
/*!

7
include/sv_mot.h
View File

@ -56,6 +56,7 @@ public:
int hits;
int misses;
int frame_id = 0;
int category_id;
bool tentative;
std::vector<double> features;
Eigen::Matrix<double, 8, 1> mean;
@ -70,6 +71,7 @@ public:
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > initiate(Eigen::Vector4d &bbox);
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > update(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances, Box &box);
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > predict(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
std::pair<Eigen::Matrix<double, 4, 1>, Eigen::Matrix<double, 4, 4> > project(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
private:
Eigen::Matrix<double, 8, 8> _F;
Eigen::Matrix<double, 4, 8> _H;
@ -88,7 +90,10 @@ public:
private:
double _iou(Tracklet &tracklet, Box &box);
std::vector<std::pair<int,int>> _hungarian(std::vector<std::vector<double>> costMatrix);
bool _augment(const std::vector<std::vector<double>>& costMatrix, int row, std::vector<int>& rowMatch, std::vector<int>& colMatch, std::vector<bool>& visited);
double _findMin(const std::vector<double>& vec);
void _subtractMinFromRows(std::vector<std::vector<double>>& costMatrix);
void _subtractMinFromCols(std::vector<std::vector<double>>& costMatrix);
//bool _augment(const std::vector<std::vector<double>>& costMatrix, int row, std::vector<int>& rowMatch, std::vector<int>& colMatch, std::vector<bool>& visited);
double _iou_threshold;
int _max_age;

36
include/sv_veri_det.h Normal file
View File

@ -0,0 +1,36 @@
#ifndef __SV_VERI_DET__
#define __SV_VERI_DET__
#include "sv_core.h"
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/tracking.hpp>
#include <string>
#include <chrono>
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<cv::Mat>& input_rois_,
std::vector<int>& output_labels_
);
VeriDetectorCUDAImpl* _cuda_impl;
};
}
#endif

27
include/sv_video_base.h
View File

@ -12,6 +12,9 @@
#include <arpa/inet.h>
#include <netinet/in.h> // for sockaddr_in
#include <mutex>
#include <condition_variable>
#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<std::string>& files, std::string suff
}
#endif

1
include/sv_world.h
View File

@ -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"

11
samples/calib/calibrate_camera_charuco.cpp
View File

@ -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;

6
samples/demo/aruco_detection.cpp
View File

@ -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;

173
samples/demo/aruco_detection_with_single_object_tracking.cpp
View File

@ -1,173 +0,0 @@
#include <iostream>
#include <string>
// 包含SpireCV SDK头文件
#include <sv_world.h>
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<tgts.targets.size(); i++)
{
printf("Frame-[%d], Aruco-[%d]\n", frame_id, i);
// 打印每个二维码的中心位置，cx，cy的值域为[0, 1]，以及cx，cy的像素值
printf(" Aruco Center (cx, cy) = (%.3f, %.3f), in Pixels = ((%d, %d))\n",
tgts.targets[i].cx, tgts.targets[i].cy,
int(tgts.targets[i].cx * tgts.width),
int(tgts.targets[i].cy * tgts.height));
// 打印每个二维码的外接矩形框的宽度、高度，w，h的值域为(0, 1]，以及w，h的像素值
printf(" Aruco Size (w, h) = (%.3f, %.3f), in Pixels = ((%d, %d))\n",
tgts.targets[i].w, tgts.targets[i].h,
int(tgts.targets[i].w * tgts.width),
int(tgts.targets[i].h * tgts.height));
// 打印每个二维码的方位角，值域为[-180, 180]
printf(" Aruco Yaw-angle = %.2f\n", tgts.targets[i].yaw_a);
// 打印每个二维码的类别，字符串类型，"aruco-?"
printf(" Aruco Category = %s\n", tgts.targets[i].category.c_str());
// 打印每个二维码的ID号
printf(" Aruco Tracked-ID = %d\n", tgts.targets[i].tracked_id);
// 打印每个二维码的视线角，跟相机视场相关
printf(" Aruco Line-of-sight (ax, ay) = (%.3f, %.3f)\n", tgts.targets[i].los_ax, tgts.targets[i].los_ay);
// 打印每个二维码的3D位置（在相机坐标系下），跟二维码实际边长、相机参数相关
printf(" Aruco 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(1);
}
return 0;
}
void onMouse(int event, int x, int y, int, void*)
{
if (b_clicked)
{
// 更新框选区域坐标
rect_sel.x = MIN(pt_origin.x, x);
rect_sel.y = MIN(pt_origin.y, y);
rect_sel.width = abs(x - pt_origin.x);
rect_sel.height = abs(y - pt_origin.y);
}
// 左键按下
if (event == cv::EVENT_LBUTTONDOWN)
{
b_begin_TRACK = false;
b_clicked = true;
pt_origin = cv::Point(x, y);
rect_sel = cv::Rect(x, y, 0, 0);
}
// 左键松开
else if (event == cv::EVENT_LBUTTONUP)
{
// 框选区域需要大于8x8像素
if (rect_sel.width * rect_sel.height < 64)
{
;
}
else
{
b_clicked = false;
b_renew_ROI = true;
}
}
}

9
samples/demo/camera_reading.cpp
View File

@ -8,9 +8,9 @@ using namespace std;
int main(int argc, char *argv[]) {
// 打开摄像头
sv::Camera cap;
cap.setWH(1280, 720);
cap.setFps(30);
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;
while (1)
@ -20,8 +20,9 @@ int main(int argc, char *argv[]) {
// 显示img
cv::imshow("img", img);
cv::waitKey(1);
cv::waitKey(10);
}
return 0;
}

193
samples/demo/car_detection_with_tracking.cpp
View File

@ -1,193 +0,0 @@
#include <iostream>
#include <string>
// 包含SpireCV SDK头文件
#include <sv_world.h>
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<tgts.targets.size(); i++)
{
printf("Frame-[%d], Object-[%d]\n", frame_id, i);
// 打印每个目标的中心位置，cx，cy的值域为[0, 1]
printf(" Object Center (cx, cy) = (%.3f, %.3f)\n", tgts.targets[i].cx, tgts.targets[i].cy);
// 打印每个目标的外接矩形框的宽度、高度，w，h的值域为(0, 1]
printf(" Object Size (w, h) = (%.3f, %.3f)\n", tgts.targets[i].w, tgts.targets[i].h);
// 打印每个目标的置信度
printf(" Object Score = %.3f\n", tgts.targets[i].score);
// 打印每个目标的类别，字符串类型
printf(" Object Category = %s, Category ID = [%d]\n", tgts.targets[i].category.c_str(), tgts.targets[i].category_id);
// 打印每个目标的视线角，跟相机视场相关
printf(" Object Line-of-sight (ax, ay) = (%.3f, %.3f)\n", tgts.targets[i].los_ax, tgts.targets[i].los_ay);
// 打印每个目标的3D位置（在相机坐标系下），跟目标实际长宽、相机参数相关
printf(" Object Position = (x, y, z) = (%.3f, %.3f, %.3f)\n", tgts.targets[i].px, tgts.targets[i].py, tgts.targets[i].pz);
p1.x = tgts.targets[i].cx * tgts.width - tgts.targets[i].w * tgts.width / 2;
p1.y = tgts.targets[i].cy * tgts.height - tgts.targets[i].h * tgts.height / 2;
p2.x = tgts.targets[i].cx * tgts.width + tgts.targets[i].w * tgts.width / 2;
p2.y = tgts.targets[i].cy * tgts.height - tgts.targets[i].h * tgts.height / 2;
p4.x = tgts.targets[i].cx * tgts.width - tgts.targets[i].w * tgts.width / 2;
p4.y = tgts.targets[i].cy * tgts.height + tgts.targets[i].h * tgts.height / 2;
p3.x = tgts.targets[i].cx * tgts.width + tgts.targets[i].w * tgts.width / 2;
p3.y = tgts.targets[i].cy * tgts.height + tgts.targets[i].h * tgts.height / 2;
p.x = pt_origin.x;
p.y = pt_origin.y;
std::cout << "p.x " << p.x << "\t" << "p.y " << p.y << std::endl;
if (getCross(p1, p2, p) * getCross(p3, p4, p) >= 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 " <<nor_x<<"/t"<<nor_p_y<< std::endl;
rect_sel = cv::Rect(p1.x, p1.y, tgts.targets[i].w * tgts.width, tgts.targets[i].h * tgts.height);
// std::cout << rect_sel << std::endl;
b_renew_ROI = true;
frame_id = 0;
printf("rect_sel Yes\n");
}
else {
printf("rect_sel No\n");
}
}
}
else {
// 实例化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);
// 可视化检测结果，叠加到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);
if (tgts.targets.size() > 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;
}
}

8
samples/demo/common_object_detection.cpp
View File

@ -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;

10
samples/demo/detection_with_clicked_tracking.cpp
View File

@ -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;

8
samples/demo/ellipse_detection.cpp
View File

@ -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;

8
samples/demo/gimbal_detection_with_clicked_tracking.cpp
View File

@ -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++;
}
}

57
samples/demo/gimbal_landing_marker_detection.cpp
View File

@ -3,7 +3,7 @@
// 包含SpireCV SDK头文件
#include <sv_world.h>
// #include "gimbal_tools.hpp"
#include <chrono>
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::nanoseconds>(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::nanoseconds>(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++;
}
}

3
samples/demo/gimbal_udp_detection_info_sender.cpp
View File

@ -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);

48
samples/demo/landing_marker_detection.cpp
View File

@ -2,7 +2,7 @@
#include <string>
// 包含SpireCV SDK头文件
#include <sv_world.h>
#include <chrono>
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::nanoseconds>(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<tgts.targets.size(); i++)
{
printf("Frame-[%d], Marker-[%d]\n", frame_id, i);
//printf("Frame-[%d], Marker-[%d]\n", frame_id, i);
// 打印每个 降落标志 的中心位置，cx，cy的值域为[0, 1]，以及cx，cy的像素值
printf(" Marker Center (cx, cy) = (%.3f, %.3f), in Pixels = ((%d, %d))\n",
tgts.targets[i].cx, tgts.targets[i].cy,
int(tgts.targets[i].cx * tgts.width),
int(tgts.targets[i].cy * tgts.height));
//printf(" Marker Center (cx, cy) = (%.3f, %.3f), in Pixels = ((%d, %d))\n",
//tgts.targets[i].cx, tgts.targets[i].cy,
//int(tgts.targets[i].cx * tgts.width),
//int(tgts.targets[i].cy * tgts.height));
// 打印每个 降落标志 的外接矩形框的宽度、高度，w，h的值域为(0, 1]，以及w，h的像素值
printf(" Marker Size (w, h) = (%.3f, %.3f), in Pixels = ((%d, %d))\n",
tgts.targets[i].w, tgts.targets[i].h,
int(tgts.targets[i].w * tgts.width),
int(tgts.targets[i].h * tgts.height));
//printf(" Marker Size (w, h) = (%.3f, %.3f), in Pixels = ((%d, %d))\n",
//tgts.targets[i].w, tgts.targets[i].h,
//int(tgts.targets[i].w * tgts.width),
//int(tgts.targets[i].h * tgts.height));
// 打印每个 降落标志 的置信度
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("img", img);
cv::waitKey(10);
}
auto time2 = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
auto Ts = time2 - time1;
std::cout << "Ts = " << Ts / (1000) << "us" << std::endl;
}
return 0;

13
samples/demo/single_object_tracking.cpp
View File

@ -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);
}
}

45
samples/demo/veri.cpp Normal file
View File

@ -0,0 +1,45 @@
#include <iostream>
#include <string>
// 包含SpireCV SDK头文件
#include <sv_world.h>
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;
}

34
samples/test/camera_fps_test.cpp Normal file
View File

@ -0,0 +1,34 @@
#include <sv_world.h>
#include <iostream>
#include <string>
#include <chrono>
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::nanoseconds>(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::nanoseconds>(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;
}
}

178
samples/test/gimbal_test.cpp Normal file
View File

@ -0,0 +1,178 @@
#include <iostream>
#include <string>
// 包含SpireCV SDK头文件
#include <sv_world.h>
#include <chrono>
// 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;
}

4
scripts/test/test_gimbal.sh Executable file
View File

@ -0,0 +1,4 @@
#!/bin/bash -e
./GimbalTest /dev/ttyUSB0 192.168.2.64

52
sv_algorithm_params.json
View File

@ -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,

184
sv_algorithm_params_coco_1280.json
View File

@ -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
}
}

184
sv_algorithm_params_coco_640.json
View File

@ -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
}
}

112
video_io/sv_video_base.cpp
View File

@ -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<std::vector<cv::Point2f> > aruco_corners;
std::vector<int> aruco_ids;
for (Target tgt : tgts_.targets)
{
if ((with_all || with_aruco) && tgt.has_aruco && (tgt.tracked_id == aruco_track_id))
{
std::vector<cv::Point2f> 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<std::mutex> 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<std::mutex> 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()
{