From 282d69c2201f5e624dba96390c9dc6d3e0daf053 Mon Sep 17 00:00:00 2001 From: xinyang <895639507@qq.com> Date: Thu, 18 Jul 2019 23:34:29 +0800 Subject: [PATCH 1/2] energy changed --- energy/src/energy/get/aim_point_get.cpp | 9 +++-- energy/src/energy/get/origin_get.cpp | 4 +- energy/src/energy/judge/judge_shoot.cpp | 6 +-- energy/src/energy/run.cpp | 54 +++++++++---------------- others/src/additions/additions.cpp | 5 ++- 5 files changed, 32 insertions(+), 46 deletions(-) diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index 4364048..c15d61d 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -13,11 +13,12 @@ using namespace cv; // --------------------------------------------------------------------------------------------------------------------- void Energy::getAimPoint(cv::Point target_point) { //五号车 - double dx = -(target_point.x - 320 - 3); - double dy = -(target_point.y - 240 - 74); +// double dx = -(target_point.x - 320 - 10); +// double dy = -(target_point.y - 240 - 22); //四号车 -// double dx = -(target_point.x - 320 + 16); -// double dy = -(target_point.y - 240 - 82); + double dx = -(target_point.x - 320 - 0); + double dy = -(target_point.y - 240 - 69); yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; + cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; } diff --git a/energy/src/energy/get/origin_get.cpp b/energy/src/energy/get/origin_get.cpp index 06f1840..415fbc9 100644 --- a/energy/src/energy/get/origin_get.cpp +++ b/energy/src/energy/get/origin_get.cpp @@ -17,8 +17,8 @@ bool Energy::getOrigin() { // double dx = -(circle_center_point.x - 320 - 10); // double dy = -(circle_center_point.y - 240 - 22); //四号车 - double dx = -(circle_center_point.x - 320 - 3); - double dy = -(circle_center_point.y - 240 - 74); + double dx = -(circle_center_point.x - 320 - 7); + double dy = -(circle_center_point.y - 240 - 64); center_delta_yaw = static_cast(atan(dx / FOCUS_PIXAL) * 180 / PI); center_delta_pitch = static_cast(atan(dy / FOCUS_PIXAL) * 180 / PI); if (abs(center_delta_yaw) > 0.3 || abs(center_delta_pitch) > 0.3) { diff --git a/energy/src/energy/judge/judge_shoot.cpp b/energy/src/energy/judge/judge_shoot.cpp index c4a50bd..1407479 100644 --- a/energy/src/energy/judge/judge_shoot.cpp +++ b/energy/src/energy/judge/judge_shoot.cpp @@ -29,7 +29,7 @@ void Energy::judgeShootInWorld(){ // 此函数用于判断云台坐标系下是否可以发弹 // --------------------------------------------------------------------------------------------------------------------- void Energy::judgeShootInGimbal(){ - if (abs(yaw_rotation) < 0.3 && fabs(pitch_rotation) < 0.3) { + if (abs(yaw_rotation) < 0.5 && fabs(pitch_rotation) < 0.5) { shoot = 4; is_predicting = false; is_guessing = true; @@ -43,6 +43,6 @@ void Energy::judgeShootInGimbal(){ static float sum_pitch = 0; sum_yaw += yaw_rotation; sum_pitch += pitch_rotation; - yaw_rotation = 2.5 * yaw_rotation + 0.1 * sum_yaw; - pitch_rotation = 2.5 * pitch_rotation + 0.1 * sum_pitch; + yaw_rotation = 6 * yaw_rotation + 0.1 * sum_yaw; + pitch_rotation = 6 * pitch_rotation + 0.1 * sum_pitch; } diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 7195b43..355db0a 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -31,7 +31,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) { if (!findCenterR(gimbal_src))return; if (show_energy)showCenterR("R", gimbal_src); - if (!getOrigin())return; +// if (!getOrigin())return; startChassis(); initEnergy(); destroyAllWindows(); @@ -60,21 +60,21 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) { initRotation(); return; } - if (is_predicting) { - getPredictPoint(target_point); - gimbalRotation(); - judgeShootInWorld(); - sendTarget(serial, yaw_rotation, pitch_rotation, shoot); - } else if (is_guessing && stayGuessing()) { - findFans(chassis_src); - if (show_energy)showFans("fans", chassis_src); - if (save_mark)writeDownMark(); - if (!guessTarget()) return; - if (show_energy)showGuessTarget("guess", chassis_src); - getPredictPoint(guess_point); - gimbalRotation(); - sendTarget(serial, yaw_rotation, pitch_rotation, 5); - } +// if (is_predicting) { +// getPredictPoint(target_point); +// gimbalRotation(); +// judgeShootInWorld(); +// sendTarget(serial, yaw_rotation, pitch_rotation, shoot); +// } else if (is_guessing && stayGuessing()) { +// findFans(chassis_src); +// if (show_energy)showFans("fans", chassis_src); +// if (save_mark)writeDownMark(); +// if (!guessTarget()) return; +// if (show_energy)showGuessTarget("guess", chassis_src); +// getPredictPoint(guess_point); +// gimbalRotation(); +// sendTarget(serial, yaw_rotation, pitch_rotation, 5); +// } } } @@ -101,11 +101,12 @@ void Energy::runBig(cv::Mat &gimbal_src) { if (show_energy)showCenterR("R", gimbal_src); changeTarget(); getTargetPolarAngle(); +// cout << target_point << '\t' << target_polar_angle << endl; // cout << circle_center_point<< endl; // judgeMode(); // if (energy_mode_init)return; -// if (!getOrigin())return; + if (!getOrigin())return; if (energy_rotation_init) { initRotation(); return; @@ -127,23 +128,6 @@ void Energy::runBig(cv::Mat &gimbal_src) { getAimPoint(predict_point); sendTarget(serial, yaw_rotation, pitch_rotation, 5); } - - if (is_predicting) { - getPredictPoint(target_point); - gimbalRotation(); - judgeShootInWorld(); - sendTarget(serial, yaw_rotation, pitch_rotation, shoot); - } else if (is_guessing && stayGuessing()) { - findFans(gimbal_src); - if (show_energy)showFans("fans", gimbal_src); - if (save_mark)writeDownMark(); - guessTarget(); - if (show_energy)showGuessTarget("guess", gimbal_src); - getPredictPoint(guess_point); - gimbalRotation(); - sendTarget(serial, yaw_rotation, pitch_rotation, 5); - } - } @@ -163,7 +147,7 @@ void Energy::runSmall(cv::Mat &gimbal_src) { if (show_energy)showFlowStripFan("strip", gimbal_src); changeTarget(); - cout << "target point: " << target_point << endl; +// cout << "target point: " << target_point << endl; if (is_predicting) { getAimPoint(target_point); diff --git a/others/src/additions/additions.cpp b/others/src/additions/additions.cpp index 5770dc0..96d43c1 100644 --- a/others/src/additions/additions.cpp +++ b/others/src/additions/additions.cpp @@ -37,8 +37,9 @@ void uartReceive(Serial *pSerial) { pSerial->ReadData((uint8_t *) buffer, sizeof(mcuData)+1); if (buffer[sizeof(mcuData)] == '\n') { memcpy(&mcuData, buffer, sizeof(mcuData)); - LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark); -// LOGM("Get yaw: %f, pitch: %f!", mcuData.curr_yaw, mcuData.curr_pitch); +// LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark); +// LOGM("Get yaw: %f, pitch: %f!ake +// ", mcuData.curr_yaw, mcuData.curr_pitch); // static int t = time(nullptr); // static int cnt = 0; // if(time(nullptr) > t){ From 0001b5ccff46a68fda86c666c17bd825735624c7 Mon Sep 17 00:00:00 2001 From: sun Date: Fri, 19 Jul 2019 11:17:45 +0800 Subject: [PATCH 2/2] energy change --- energy/src/energy/calibrate/structing.cpp | 6 ++-- energy/src/energy/clear/energy_init.cpp | 20 ++++++------ energy/src/energy/find/energy_finder.cpp | 14 ++++++--- energy/src/energy/judge/judge_contour.cpp | 37 +++++++++++++++++------ main.cpp | 6 ++-- others/src/additions/additions.cpp | 2 +- 6 files changed, 53 insertions(+), 32 deletions(-) diff --git a/energy/src/energy/calibrate/structing.cpp b/energy/src/energy/calibrate/structing.cpp index 721a282..96facbb 100644 --- a/energy/src/energy/calibrate/structing.cpp +++ b/energy/src/energy/calibrate/structing.cpp @@ -125,7 +125,7 @@ void Energy::FlowStripStruct(cv::Mat &src) { // 此函数对图像进行腐蚀与膨胀操作 // --------------------------------------------------------------------------------------------------------------------- void Energy::CenterRStruct(cv::Mat &src) { - Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(8, 6)); + Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(4, 4)); Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 1)); Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3)); Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4 , 4)); @@ -134,8 +134,8 @@ void Energy::CenterRStruct(cv::Mat &src) { // imshow("erode_1", src); dilate(src, src, element_dilate_1); // imshow("dilate_1", src); - dilate(src, src, element_dilate_2); +// dilate(src, src, element_dilate_2); // imshow("dilate_2", src); - erode(src,src, element_erode_2); +// erode(src,src, element_erode_2); // imshow("erode_2", src); } \ No newline at end of file diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index fe76569..e91b640 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -79,7 +79,7 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 80; gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100; gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 20; - gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 50; + gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52; gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4; gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1; gimbal_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.65; @@ -105,7 +105,7 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 25; gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1; - gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.7; + gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.5; gimbal_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 3000; @@ -113,7 +113,7 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20; - gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 50; + gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 52; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.65; @@ -125,14 +125,14 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 100000; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 0; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 45; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 70; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 12; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 24; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 38; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 60; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 8; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 32; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12; // gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 2.5; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.6; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 1.8; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.5; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100; gimbal_energy_part_param_.TWIN_ANGEL_MAX = 10; @@ -187,7 +187,7 @@ void Energy::initEnergyPartParam() { chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 90; chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 140; chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 35; - chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 60; + chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 65; chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 3; chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1; chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.55; diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index f9dcf8e..8767ff4 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -133,7 +133,7 @@ bool Energy::findCenterR(const cv::Mat src) { circle_center_point = centerR.center; circle_center_point.y += target_length / 7.5;//实际最小二乘得到的中心在R的下方 -// RotatedRect cur_rect = minAreaRect(center_R_contour); + RotatedRect cur_rect = minAreaRect(center_R_contour); // Size2f cur_size = cur_rect.size; // float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; @@ -148,10 +148,11 @@ bool Energy::findCenterR(const cv::Mat src) { // cout << "R intersection: " << contourArea(intersection) << endl; // return true; // } - +// cout << cur_rect.center << endl; return true; } cout << "find center R false!" << endl; +// cv::waitKey(0); return false; } @@ -160,7 +161,7 @@ bool Energy::findCenterR(const cv::Mat src) { //---------------------------------------------------------------------------------------------------------------------- // 此函数用于判断找到的矩形候选区是否为含流动条的扇叶 // --------------------------------------------------------------------------------------------------------------------- -bool Energy:: findFlowStripFan(const cv::Mat src) { +bool Energy::findFlowStripFan(const cv::Mat src) { if (src.empty())return false; static Mat src_bin; static Mat src_copy; @@ -171,7 +172,7 @@ bool Energy:: findFlowStripFan(const cv::Mat src) { } std::vector > flow_strip_fan_contours; FlowStripFanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 -// imshow("flow strip fan struct", src_bin); + imshow("flow strip fan struct", src_bin); findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); @@ -194,11 +195,12 @@ bool Energy:: findFlowStripFan(const cv::Mat src) { // cout << "non zero: " << non_zero_rate << endl; // cout< &fan_contou if (length < energy_part_param_.FAN_CONTOUR_LENGTH_MIN || width < energy_part_param_.FAN_CONTOUR_WIDTH_MIN || length > energy_part_param_.FAN_CONTOUR_LENGTH_MAX || width > energy_part_param_.FAN_CONTOUR_WIDTH_MAX) { //cout<<"length width fail."< &fan_contou if (length_width_ratio > energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX || length_width_ratio < energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN) { //cout<<"length width ratio fail."< &armor_contour) { if (length < energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN || width < energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN || length > energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX || width > energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX) { //cout<<"length width fail."< &armor_contour) { if (length_width_ratio > energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX || length_width_ratio < energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN) { //cout<<"length width ratio fail."< ¢er_R_contour) { || length > energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX || width > energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX) { //cout<<"length width fail."< ¢er_R_contour) { if (length_width_ratio > energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX || length_width_ratio < energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN) { //cout<<"length width ratio fail."< intersection; if (rotatedRectangleIntersection(cur_rect, center_ROI, intersection) == 0) { return false; } else if (contourArea(intersection) < energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN) { - cout << "R intersection: " << contourArea(intersection) << endl; +// cout << "R intersection: " << contourArea(intersection) << '\t' << cur_rect.center << endl; return false; } return true; @@ -138,8 +149,8 @@ bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector &f double cur_contour_area = contourArea(flow_strip_fan_contour); if (cur_contour_area > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX || cur_contour_area < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN) { - //cout< &f || length > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX || width > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX) { // cout<<"length width fail."< &f if (length_width_ratio > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX || length_width_ratio < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN) { //cout<<"length width ratio fail."< energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX) { -// cout << "rate: " << cur_contour_area / cur_size.area() << endl; +// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; return false; } // 轮廓对矩形的面积占有率不合适 @@ -204,7 +216,7 @@ bool Energy::isValidFlowStripContour(const vector &flow_strip_contour length > energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX || width > energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX) { // cout<<"length width fail."< &flow_strip_contour if (length_width_ratio > energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX || length_width_ratio < energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN) { // cout<<"hw fail."< intersection; - if (rotatedRectangleIntersection(cur_rect, flow_strip_fan, intersection) == 0 || - contourArea(intersection) < energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) { + if (rotatedRectangleIntersection(cur_rect, flow_strip_fan, intersection) == 0) { + return false; + } else if (contourArea(intersection) < energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) { +// cout << "intersection: " << contourArea(intersection) << '\t' << cur_rect.center << endl; return false; } return true; diff --git a/main.cpp b/main.cpp index ca68173..e0662c5 100644 --- a/main.cpp +++ b/main.cpp @@ -64,8 +64,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(0/*, "armor"*/); video_chassis = new CameraWrapper(1/*, "energy"*/); } else { - video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi"); - video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi"); + video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimbal106.avi"); + video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimbal106.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); @@ -144,7 +144,7 @@ int main(int argc, char *argv[]) { armorFinder.run(gimbal_src); }); } -// cv::waitKey(1); +// cv::waitKey(0); // }); } while (ok); delete video_gimbal; diff --git a/others/src/additions/additions.cpp b/others/src/additions/additions.cpp index 5770dc0..19dd316 100644 --- a/others/src/additions/additions.cpp +++ b/others/src/additions/additions.cpp @@ -37,7 +37,7 @@ void uartReceive(Serial *pSerial) { pSerial->ReadData((uint8_t *) buffer, sizeof(mcuData)+1); if (buffer[sizeof(mcuData)] == '\n') { memcpy(&mcuData, buffer, sizeof(mcuData)); - LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark); +// LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark); // LOGM("Get yaw: %f, pitch: %f!", mcuData.curr_yaw, mcuData.curr_pitch); // static int t = time(nullptr); // static int cnt = 0;