tangchao0503/Payload-SDK
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

NEW: release DJI Payload-SDK version 3.12.0

Browse Source
This commit is contained in:
DJI
2025-06-27 22:36:34 +08:00
parent 54b9f6c6c1
commit 326b8698dd
381 changed files with 122574 additions and 451 deletions
Download Patch File Download Diff File Expand all files Collapse all files

272
samples/sample_c++/module_sample/perception/test_lidar_entry.cpp Normal file
View File

@ -0,0 +1,272 @@
/**
********************************************************************
* @file test_lidar_entry.cpp
* @brief
*
* @copyright (c) 2021 DJI. All rights reserved.
*
* All information contained herein is, and remains, the property of DJI.
* The intellectual and technical concepts contained herein are proprietary
* to DJI and may be covered by U.S. and foreign patents, patents in process,
* and protected by trade secret or copyright law. Dissemination of this
* information, including but not limited to data and other proprietary
* material(s) incorporated within the information, in any form, is strictly
* prohibited without the express written consent of DJI.
*
* If you receive this source code without DJI’s authorization, you may not
* further disseminate the information, and you must immediately remove the
* source code and notify DJI of its removal. DJI reserves the right to pursue
* legal actions against you for any loss(es) or damage(s) caused by your
* failure to do so.
*
*********************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "test_lidar_entry.hpp"
#include <dirent.h>
#include "dji_logger.h"
#include <iostream>
#include <fstream>
#include <string>
#include <ctime>
#include <mutex>
#include <fcntl.h>
#include <unistd.h>
#include <mutex>
#include <thread>
#include <iomanip>
#include <sstream>
#include <sys/stat.h>
#include <sys/types.h>
#include <queue>
/* Private constants ---------------------------------------------------------*/
#define PCD_FILE_DEFAULT_LENGTH (512)
#define FRAME_BUFFER_LENGTH (1024 * 1024)
#define SUBSCRIBE_DATA_TIME_MS (1000 * 10)
#define USER_PERCEPTION_LIRDAR_TASK_STACK_SIZE (2042)
#define PCD_FILE_PATH "./DJI_cloud_data"
/* Private types -------------------------------------------------------------*/
/* Private values -------------------------------------------------------------*/
static int lastFrameCnt = 0;
static std::queue<T_DjiLidarFrame *> lidarFrameQueue;
static T_DjiMutexHandle queueMutex;
static T_DjiSemaHandle dataSemaphore;
static bool stopProcessing = false;
static T_DjiSemaHandle taskExitSema;
/* Private functions declaration ---------------------------------------------*/
static void DjiTest_PerceptionLidarCallback(uint8_t *recvBuffer, uint32_t bufferLen);
static std::string DjiTest_getCurrentTimestamp();
static void DjiTest_WriteLidarFrameToBinaryPcdFile(const T_DjiLidarFrame *frame);
static void* DjiTest_ProcessLidarDataTask(void* arg);
/* Exported functions definition ---------------------------------------------*/
void DjiUser_RunLidarDataSubscriptionSample(void) {
int subscriptionDuration = 10;
lastFrameCnt = 0;
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
osalHandler->MutexCreate(&queueMutex);
osalHandler->SemaphoreCreate(0, &dataSemaphore);
osalHandler->SemaphoreCreate(0, &taskExitSema);
std::cout << "Please ensure that there is enough storage space for the PCD files." << std::endl;
T_DjiTaskHandle processingThread;
osalHandler->TaskCreate("LidarProcessingThread", DjiTest_ProcessLidarDataTask, USER_PERCEPTION_LIRDAR_TASK_STACK_SIZE, nullptr, &processingThread);
start:
T_DjiReturnCode returnCode;
returnCode = DjiPerception_Init();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
std::cout << "DjiPerception Init failed" << std::endl;
return;
}
std::cout << "start subscribe Lidar data from aircraft" << std::endl;
returnCode = DjiPerception_SubscribeLidarData(DjiTest_PerceptionLidarCallback);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
std::cout << "Request to subscribe Lidar data failed" << std::endl;
goto subscribeFailed;
}
osalHandler->TaskSleepMs(subscriptionDuration * 1000);
std::cout << "unsubscribe Lidar data " << std::endl;
subscribeFailed:
returnCode = DjiPerception_UnsubscribeLidarData();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
std::cout << "Request to unsubscribe Lidar data failed" << std::endl;
}
returnCode = DjiPerception_Deinit();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
std::cout << "DjiPerception DeInit failed" << std::endl;
return;
}
std::cout << "unsubscribe Lidar data success" << std::endl;
osalHandler->MutexLock(queueMutex);
stopProcessing = true;
osalHandler->MutexUnlock(queueMutex);
osalHandler->SemaphorePost(dataSemaphore);
osalHandler->SemaphoreWait(taskExitSema);
osalHandler->TaskDestroy(processingThread);
osalHandler->MutexDestroy(queueMutex);
osalHandler->SemaphoreDestroy(dataSemaphore);
osalHandler->SemaphoreDestroy(taskExitSema);
}
/* Private functions definition-----------------------------------------------*/
static void DjiTest_PerceptionLidarCallback(uint8_t *LidarFrame, uint32_t bufferLen) {
if (bufferLen != sizeof(T_DjiLidarFrame)) {
std::cout << "usb recv Lidar length wrong, length = " << bufferLen << std::endl;
return;
}
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
T_DjiLidarFrame * curFrame = (T_DjiLidarFrame *)osalHandler->Malloc(bufferLen);
memcpy(curFrame, LidarFrame, bufferLen);
osalHandler->MutexLock(queueMutex);
lidarFrameQueue.push(curFrame);
osalHandler->MutexUnlock(queueMutex);
osalHandler->SemaphorePost(dataSemaphore);
}
std::string DjiTest_getCurrentTimestamp() {
auto now = std::chrono::system_clock::now();
std::time_t nowTimeT = std::chrono::system_clock::to_time_t(now);
auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
std::tm nowTm = *std::localtime(&nowTimeT);
std::ostringstream oss;
oss << std::put_time(&nowTm, "%Y%m%d_%H%M%S");
oss << std::setfill('0') << std::setw(3) << milliseconds.count();
return oss.str();
}
static void DjiTest_WriteLidarFrameToBinaryPcdFile(const T_DjiLidarFrame *frame) {
uint32_t totalPoints = 0;
size_t headerLen = 0;
size_t pointDataSize = 0;
size_t bufferSize = 0;
char *buffer = NULL;
size_t bufferPos = 0;
int fd = 0;
std::string directory = PCD_FILE_PATH;
std::string filename = directory + "/DJI_cloud_data_" + DjiTest_getCurrentTimestamp() + ".pcd";
char header[PCD_FILE_DEFAULT_LENGTH];
if (mkdir(directory.c_str(), 0755) != 0 && errno != EEXIST) {
fprintf(stderr, "Error creating directory: %s\n", strerror(errno));
return;
}
for (uint16_t i = 0; i < frame->pkgNum; ++i) {
totalPoints += frame->pkgs[i].header.dotNum;
}
snprintf(header, sizeof(header),
"# .PCD v0.7 - Point Cloud Data file format\n"
"VERSION 0.7\n"
"FIELDS x y z intensity label\n"
"SIZE 4 4 4 1 1\n"
"TYPE F F F U U\n"
"COUNT 1 1 1 1 1\n"
"WIDTH %u\n"
"HEIGHT 1\n"
"VIEWPOINT 0 0 0 1 0 0 0\n"
"POINTS %u\n"
"DATA binary\n",
totalPoints, totalPoints);
// Calculate the size of the buffer needed
headerLen = strlen(header);
pointDataSize = totalPoints * (sizeof(float) * 3 + sizeof(uint8_t) * 2);
bufferSize = headerLen + pointDataSize;
buffer = (char *)malloc(bufferSize);
if (buffer == NULL) {
fprintf(stderr, "Error allocating memory for buffer\n");
return;
}
memcpy(buffer + bufferPos, header, headerLen);
bufferPos += headerLen;
for (uint16_t i = 0; i < frame->pkgNum; ++i) {
const T_DjiPerceptionLidarDecodePkg *pkg = &frame->pkgs[i];
for (uint16_t j = 0; j < pkg->header.dotNum; ++j) {
const T_DJIPerceptionLidarPoint *point = &pkg->points[j];
memcpy(buffer + bufferPos, &point->x, sizeof(float));
bufferPos += sizeof(float);
memcpy(buffer + bufferPos, &point->y, sizeof(float));
bufferPos += sizeof(float);
memcpy(buffer + bufferPos, &point->z, sizeof(float));
bufferPos += sizeof(float);
memcpy(buffer + bufferPos, &point->intensity, sizeof(uint8_t));
bufferPos += sizeof(uint8_t);
memcpy(buffer + bufferPos, &point->label, sizeof(uint8_t));
bufferPos += sizeof(uint8_t);
}
}
fd = open(filename.c_str(), O_WRONLY | O_APPEND | O_CREAT, 0644);
if (fd == -1) {
fprintf(stderr, "Error opening file for writing\n");
free(buffer);
return;
}
if (write(fd, buffer, bufferSize) == -1) {
fprintf(stderr, "Error writing buffer to file\n");
}
close(fd);
free(buffer);
}
static void* DjiTest_ProcessLidarDataTask(void* arg) {
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
while(true) {
osalHandler->SemaphoreWait(dataSemaphore);
osalHandler->MutexLock(queueMutex);
bool shouldStop = stopProcessing && lidarFrameQueue.empty();
if(shouldStop) {
osalHandler->MutexUnlock(queueMutex);
break;
}
T_DjiLidarFrame *lidarFrame = lidarFrameQueue.front();
lidarFrameQueue.pop();
osalHandler->MutexUnlock(queueMutex);
DjiTest_WriteLidarFrameToBinaryPcdFile(lidarFrame);
int curFrameCnt = lidarFrame->frameCnt;
std::cout << "Lidar data : curFrameCnt=" << curFrameCnt << std::endl;
if(lastFrameCnt != 0 && (curFrameCnt - lastFrameCnt) > 1) {
std::cout << "The number of lost packets during transmission is: " << curFrameCnt - lastFrameCnt - 1 << std::endl;
}
lastFrameCnt = curFrameCnt;
osalHandler->Free(lidarFrame);
}
osalHandler->SemaphorePost(taskExitSema);
return nullptr;
}
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/

49
samples/sample_c++/module_sample/perception/test_lidar_entry.hpp Normal file
View File

@ -0,0 +1,49 @@
/**
********************************************************************
* @file test_lidar_entry.hpp
* @brief This is the header file for "test_lidar_entry.cpp", defining the structure and
* (exported) function prototypes.
*
* @copyright (c) 2021 DJI. All rights reserved.
*
* All information contained herein is, and remains, the property of DJI.
* The intellectual and technical concepts contained herein are proprietary
* to DJI and may be covered by U.S. and foreign patents, patents in process,
* and protected by trade secret or copyright law. Dissemination of this
* information, including but not limited to data and other proprietary
* material(s) incorporated within the information, in any form, is strictly
* prohibited without the express written consent of DJI.
*
* If you receive this source code without DJI’s authorization, you may not
* further disseminate the information, and you must immediately remove the
* source code and notify DJI of its removal. DJI reserves the right to pursue
* legal actions against you for any loss(es) or damage(s) caused by your
* failure to do so.
*
*********************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef TEST_LIDAR_ENTRY_H
#define TEST_LIDAR_ENTRY_H
/* Includes ------------------------------------------------------------------*/
#include "dji_perception.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
void DjiUser_RunLidarDataSubscriptionSample(void);
#ifdef __cplusplus
}
#endif
#endif // TEST_LIDAR_ENTRY_H
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/

190
samples/sample_c++/module_sample/perception/test_radar_entry.cpp Normal file
View File

@ -0,0 +1,190 @@
/**
********************************************************************
* @file test_radar_entry.cpp
* @brief
*
* @copyright (c) 2021 DJI. All rights reserved.
*
* All information contained herein is, and remains, the property of DJI.
* The intellectual and technical concepts contained herein are proprietary
* to DJI and may be covered by U.S. and foreign patents, patents in process,
* and protected by trade secret or copyright law. Dissemination of this
* information, including but not limited to data and other proprietary
* material(s) incorporated within the information, in any form, is strictly
* prohibited without the express written consent of DJI.
*
* If you receive this source code without DJI’s authorization, you may not
* further disseminate the information, and you must immediately remove the
* source code and notify DJI of its removal. DJI reserves the right to pursue
* legal actions against you for any loss(es) or damage(s) caused by your
* failure to do so.
*
*********************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "test_radar_entry.hpp"
#include "dji_logger.h"
#include <iostream>
#include <ctime>
#include <chrono>
/* Private constants ---------------------------------------------------------*/
/* Private types -------------------------------------------------------------*/
/* Private values -------------------------------------------------------------*/
/* Private functions declaration ---------------------------------------------*/
static void DjiTest_PerceptionRadarCallback(E_DjiPerceptionRadarPosition radarPosition,
uint8_t *radarDataBuffer, uint32_t bufferLen);
static float parseVelocity(uint16_t velocity);
static float parseBeamAngle(uint16_t beamAngle);
/* Exported functions definition ---------------------------------------------*/
void DjiUser_RunRadarDataSubscriptionSample(void) {
int subscriptionDuration = 10;
T_DjiReturnCode returnCode;
char inputChar;
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
E_DjiPerceptionRadarPosition curPosition = MAX_RADAR_NUM;
returnCode = DjiPerception_Init();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("DjiPerception Init failed");
return;
}
inputAgain:
std::cout
<< "| Available commands: |"
<<
std::endl;
std::cout
<< "| [d] Subscribe to downward millimeter wave radar data. |"
<<
std::endl;
std::cout
<< "| [u] Subscribe to upward millimeter wave radar data. |"
<<
std::endl;
std::cout
<< "| [f] Subscribe to forward millimeter wave radar data. |"
<<
std::endl;
std::cout
<< "| [b] Subscribe to backward millimeter wave radar data. |"
<<
std::endl;
std::cout
<< "| [l] Subscribe to leftward millimeter wave radar data. |"
<<
std::endl;
std::cout
<< "| [r] Subscribe to rightward millimeter wave radar data. |"
<<
std::endl;
std::cout
<< "| [q] quit |"
<<
std::endl;
std::cin >> inputChar;
switch (inputChar) {
case 'd':
USER_LOG_INFO("Subscribe to downward millimeter wave radar data");
curPosition = RADAR_POSITION_DOWN;
break;
case 'u':
USER_LOG_INFO("Subscribe to upward millimeter wave radar data.");
curPosition = RADAR_POSITION_UP;
break;
case 'f':
USER_LOG_INFO("Subscribe to forward millimeter wave radar data.");
curPosition = RADAR_POSITION_FRONT;
break;
case 'b':
USER_LOG_INFO("Subscribe to backward millimeter wave radar data.");
curPosition = RADAR_POSITION_BACK;
break;
case 'l':
USER_LOG_INFO("Subscribe to leftward millimeter wave radar data.");
curPosition = RADAR_POSITION_LEFT;
break;
case 'r':
USER_LOG_INFO("Subscribe to rightward millimeter wave radar data.");
curPosition = RADAR_POSITION_RIGHT;
break;
case 'q':
goto endOfSample;
default:
goto inputAgain;
}
returnCode = DjiPerception_SubscribeRadarData(curPosition, DjiTest_PerceptionRadarCallback);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Request to subscribe radar data failed");
goto subscribeFailed;
}
osalHandler->TaskSleepMs(subscriptionDuration * 1000);
subscribeFailed:
returnCode = DjiPerception_UnsubscribeRadarData(curPosition);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Request to unsubscribe Radar data failed");
}
goto inputAgain;
endOfSample:
returnCode = DjiPerception_Deinit();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("DjiPerception DeInit failed");
return;
}
}
/* Private functions definition-----------------------------------------------*/
static void DjiTest_PerceptionRadarCallback(E_DjiPerceptionRadarPosition radarPosition,
uint8_t *radarDataBuffer, uint32_t bufferLen) {
T_DjiRadarDataFrame* radarData;
radarData = (T_DjiRadarDataFrame*)radarDataBuffer;
if (radarDataBuffer == nullptr || bufferLen == 0) {
USER_LOG_ERROR("Invalid radar data: buffer=%p len=%u", radarDataBuffer, bufferLen);
return;
}
USER_LOG_INFO("RadarData[pos:%d][len:%u][units:%u][pack:%u/%u]",
radarPosition,
bufferLen,
static_cast<unsigned>(radarData->headInfo.dataLen),
static_cast<unsigned>(radarData->headInfo.curPack),
static_cast<unsigned>(radarData->headInfo.packNum));
for (uint32_t i = 0; i < radarData->headInfo.dataLen; ++i) {
const T_DjiRadarCloudUnit* unit = &radarData->data[i];
float azimuth = unit->azimuth / 1000.0f - 2*3.14;
float elevation = unit->elevation / 1000.0f - 2*3.14;
float radius = unit->radius / 100.0f;
float energy = unit->ene / 100.0f;
float velocity = parseVelocity(unit->base_info.velocity);
uint8_t snr = unit->base_info.snr;
float beamAngle = parseBeamAngle(unit->base_info.beamAngle);
USER_LOG_INFO(
"[Unit%d] Azimuth=%.3f(rad), Elevation=%.3f(rad), Radius=%.2f(m), Energy=%.2f, "
"Velocity=%.2f(m/s), SNR=%u(dB), BeamAngle=%.2f(deg)",
i, azimuth, elevation, radius, energy, velocity, snr, beamAngle);
}
}
static float parseVelocity(uint16_t velocity) {
return (velocity - 32767) / 100.0f;
}
static float parseBeamAngle(uint16_t beamAngle) {
if (beamAngle <= 450) {
return beamAngle / 10.0f;
} else {
return (beamAngle / 10.0f) - 90.0f;
}
}
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/

49
samples/sample_c++/module_sample/perception/test_radar_entry.hpp Normal file
View File

@ -0,0 +1,49 @@
/**
********************************************************************
* @file test_radar_entry.hpp
* @brief This is the header file for "test_radar_entry.cpp", defining the structure and
* (exported) function prototypes.
*
* @copyright (c) 2021 DJI. All rights reserved.
*
* All information contained herein is, and remains, the property of DJI.
* The intellectual and technical concepts contained herein are proprietary
* to DJI and may be covered by U.S. and foreign patents, patents in process,
* and protected by trade secret or copyright law. Dissemination of this
* information, including but not limited to data and other proprietary
* material(s) incorporated within the information, in any form, is strictly
* prohibited without the express written consent of DJI.
*
* If you receive this source code without DJI’s authorization, you may not
* further disseminate the information, and you must immediately remove the
* source code and notify DJI of its removal. DJI reserves the right to pursue
* legal actions against you for any loss(es) or damage(s) caused by your
* failure to do so.
*
*********************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef TEST_RADAR_ENTRY_H
#define TEST_RADAR_ENTRY_H
/* Includes ------------------------------------------------------------------*/
#include "dji_perception.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
void DjiUser_RunRadarDataSubscriptionSample(void);
#ifdef __cplusplus
}
#endif
#endif // TEST_RADAR_ENTRY_H
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/