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

NEW: release DJI Payload-SDK version 3.0

Browse Source 
Signed-off-by: DJI-Martin <DJI-Martin@dji.com>
This commit is contained in:
DJI-Martin
2021-11-12 16:27:03 +08:00
parent 2cce11a80b
commit e478e9a7a5
541 changed files with 2192778 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

329
samples/sample_c/module_sample/fc_subscription/test_fc_subscription.c Normal file
View File

@ -0,0 +1,329 @@
/**
********************************************************************
* @file test_fc_subscription.c
* @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 <utils/util_misc.h>
#include <math.h>
#include "test_fc_subscription.h"
#include "dji_logger.h"
#include "dji_platform.h"
#include "widget_interaction_test/test_widget_interaction.h"
/* Private constants ---------------------------------------------------------*/
#define FC_SUBSCRIPTION_TASK_FREQ (1)
#define FC_SUBSCRIPTION_TASK_STACK_SIZE (2048)
/* Private types -------------------------------------------------------------*/
/* Private functions declaration ---------------------------------------------*/
static void *UserFcSubscription_Task(void *arg);
static T_DjiReturnCode DjiTest_FcSubscriptionReceiveQuaternionCallback(const uint8_t *data, uint16_t dataSize,
const T_DjiDataTimestamp *timestamp);
/* Private variables ---------------------------------------------------------*/
static T_DjiTaskHandle s_userFcSubscriptionThread;
static bool s_userFcSubscriptionDataShow = false;
static uint8_t s_totalSatelliteNumberUsed = 0;
/* Exported functions definition ---------------------------------------------*/
T_DjiReturnCode DjiTest_FcSubscriptionStartService(void)
{
T_DjiReturnCode djiStat;
T_DjiOsalHandler *osalHandler = NULL;
osalHandler = DjiPlatform_GetOsalHandler();
djiStat = DjiFcSubscription_Init();
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("init data subscription module error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
djiStat = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION, DJI_DATA_SUBSCRIPTION_TOPIC_10_HZ,
DjiTest_FcSubscriptionReceiveQuaternionCallback);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Subscribe topic quaternion error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
} else {
USER_LOG_DEBUG("Subscribe topic quaternion success.");
}
djiStat = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
NULL);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Subscribe topic velocity error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
} else {
USER_LOG_DEBUG("Subscribe topic velocity success.");
}
djiStat = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
NULL);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Subscribe topic gps position error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
} else {
USER_LOG_DEBUG("Subscribe topic gps position success.");
}
djiStat = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DETAILS, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
NULL);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Subscribe topic gps details error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
} else {
USER_LOG_DEBUG("Subscribe topic gps details success.");
}
if (osalHandler->TaskCreate("user_subscription_task", UserFcSubscription_Task,
FC_SUBSCRIPTION_TASK_STACK_SIZE, NULL, &s_userFcSubscriptionThread) !=
DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("user data subscription task create error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
T_DjiReturnCode DjiTest_FcSubscriptionRunSample(void)
{
T_DjiReturnCode djiStat;
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
T_DjiFcSubscriptionVelocity velocity = {0};
T_DjiDataTimestamp timestamp = {0};
T_DjiFcSubscriptionGpsPosition gpsPosition = {0};
T_DjiFcSubscriptionSingleBatteryInfo singleBatteryInfo = {0};
USER_LOG_INFO("Fc subscription sample start");
USER_LOG_INFO("--> Step 1: Init fc subscription module");
djiStat = DjiFcSubscription_Init();
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("init data subscription module error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
USER_LOG_INFO("--> Step 2: Subscribe the topics of quaternion, velocity and gps position");
djiStat = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION, DJI_DATA_SUBSCRIPTION_TOPIC_10_HZ,
DjiTest_FcSubscriptionReceiveQuaternionCallback);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Subscribe topic quaternion error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
djiStat = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
NULL);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Subscribe topic velocity error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
djiStat = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
NULL);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Subscribe topic gps position error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
USER_LOG_INFO("--> Step 3: Get latest value of the subscribed topics in the next 20s\r\n");
for (int i = 0; i < 20; ++i) {
osalHandler->TaskSleepMs(1000 / FC_SUBSCRIPTION_TASK_FREQ);
djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY,
(uint8_t *) &velocity,
sizeof(T_DjiFcSubscriptionVelocity),
&timestamp);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("get value of topic velocity error.");
} else {
USER_LOG_INFO("velocity: x = %f y = %f z = %f healthFlag = %d.", velocity.data.x, velocity.data.y,
velocity.data.z, velocity.health);
}
djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION,
(uint8_t *) &gpsPosition,
sizeof(T_DjiFcSubscriptionGpsPosition),
&timestamp);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("get value of topic gps position error.");
} else {
USER_LOG_INFO("gps position: x = %d y = %d z = %d.", gpsPosition.x, gpsPosition.y, gpsPosition.z);
}
//Attention: if you want to subscribe the single battery info on M300 RTK, you need connect USB cable to
//OSDK device or use topic DJI_FC_SUBSCRIPTION_TOPIC_BATTERY_INFO instead.
djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_BATTERY_SINGLE_INFO_INDEX1,
(uint8_t *) &singleBatteryInfo,
sizeof(T_DjiFcSubscriptionSingleBatteryInfo),
&timestamp);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("get value of topic battery single info index1 error.");
} else {
USER_LOG_INFO(
"battery single info index1: capacity percent = %ld% voltage = %ldV temperature = %.2f degree.",
singleBatteryInfo.batteryCapacityPercent,
singleBatteryInfo.currentVoltage / 1000,
(dji_f32_t) singleBatteryInfo.batteryTemperature / 10);
}
djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_BATTERY_SINGLE_INFO_INDEX2,
(uint8_t *) &singleBatteryInfo,
sizeof(T_DjiFcSubscriptionSingleBatteryInfo),
&timestamp);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("get value of topic battery single info index2 error.");
} else {
USER_LOG_INFO(
"battery single info index2: capacity percent = %ld% voltage = %ldV temperature = %.2f degree.\r\n",
singleBatteryInfo.batteryCapacityPercent,
singleBatteryInfo.currentVoltage / 1000,
(dji_f32_t) singleBatteryInfo.batteryTemperature / 10);
}
}
USER_LOG_INFO("--> Step 4: Deinit fc subscription module");
djiStat = DjiFcSubscription_DeInit();
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Deinit fc subscription error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
USER_LOG_INFO("Fc subscription sample end");
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
T_DjiReturnCode DjiTest_FcSubscriptionDataShowTrigger(void)
{
s_userFcSubscriptionDataShow = !s_userFcSubscriptionDataShow;
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
T_DjiReturnCode DjiTest_FcSubscriptionGetTotalSatelliteNumber(uint8_t *number)
{
*number = s_totalSatelliteNumberUsed;
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
/* Private functions definition-----------------------------------------------*/
#ifndef __CC_ARM
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmissing-noreturn"
#pragma GCC diagnostic ignored "-Wreturn-type"
#endif
static void *UserFcSubscription_Task(void *arg)
{
T_DjiReturnCode djiStat;
T_DjiFcSubscriptionVelocity velocity = {0};
T_DjiDataTimestamp timestamp = {0};
T_DjiFcSubscriptionGpsPosition gpsPosition = {0};
T_DjiFcSubscriptionGpsDetails gpsDetails = {0};
T_DjiOsalHandler *osalHandler = NULL;
USER_UTIL_UNUSED(arg);
osalHandler = DjiPlatform_GetOsalHandler();
while (1) {
osalHandler->TaskSleepMs(1000 / FC_SUBSCRIPTION_TASK_FREQ);
djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY,
(uint8_t *) &velocity,
sizeof(T_DjiFcSubscriptionVelocity),
&timestamp);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("get value of topic velocity error.");
}
if (s_userFcSubscriptionDataShow == true) {
USER_LOG_INFO("velocity: x %f y %f z %f, healthFlag %d.", velocity.data.x, velocity.data.y,
velocity.data.z, velocity.health);
}
djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION,
(uint8_t *) &gpsPosition,
sizeof(T_DjiFcSubscriptionGpsPosition),
&timestamp);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("get value of topic gps position error.");
}
if (s_userFcSubscriptionDataShow == true) {
USER_LOG_INFO("gps position: x %d y %d z %d.", gpsPosition.x, gpsPosition.y, gpsPosition.z);
}
djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DETAILS,
(uint8_t *) &gpsDetails,
sizeof(T_DjiFcSubscriptionGpsDetails),
&timestamp);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("get value of topic gps details error.");
}
if (s_userFcSubscriptionDataShow == true) {
USER_LOG_INFO("gps total satellite number used: %d %d %d.",
gpsDetails.gpsSatelliteNumberUsed,
gpsDetails.glonassSatelliteNumberUsed,
gpsDetails.totalSatelliteNumberUsed);
s_totalSatelliteNumberUsed = gpsDetails.totalSatelliteNumberUsed;
}
}
}
#ifndef __CC_ARM
#pragma GCC diagnostic pop
#endif
static T_DjiReturnCode DjiTest_FcSubscriptionReceiveQuaternionCallback(const uint8_t *data, uint16_t dataSize,
const T_DjiDataTimestamp *timestamp)
{
T_DjiFcSubscriptionQuaternion *quaternion = (T_DjiFcSubscriptionQuaternion *) data;
dji_f64_t pitch, yaw, roll;
USER_UTIL_UNUSED(dataSize);
pitch = (dji_f64_t) asinf(-2 * quaternion->q1 * quaternion->q3 + 2 * quaternion->q0 * quaternion->q2) * 57.3;
roll = (dji_f64_t) atan2f(2 * quaternion->q1 * quaternion->q2 + 2 * quaternion->q0 * quaternion->q3,
-2 * quaternion->q2 * quaternion->q2 - 2 * quaternion->q3 * quaternion->q3 + 1) *
57.3;
yaw = (dji_f64_t) atan2f(2 * quaternion->q2 * quaternion->q3 + 2 * quaternion->q0 * quaternion->q1,
-2 * quaternion->q1 * quaternion->q1 - 2 * quaternion->q2 * quaternion->q2 + 1) * 57.3;
if (s_userFcSubscriptionDataShow == true) {
USER_LOG_INFO("receive quaternion data.");
USER_LOG_INFO("timestamp: millisecond %u microsecond %u.", timestamp->millisecond,
timestamp->microsecond);
USER_LOG_INFO("quaternion: %f %f %f %f.\r\n", quaternion->q0, quaternion->q1, quaternion->q2, quaternion->q3);
USER_LOG_INFO("euler angles: pitch = %.2f roll = %.2f yaw = %.2f.", pitch, yaw, roll);
DjiTest_WidgetLogAppend("pitch = %.2f roll = %.2f yaw = %.2f.", pitch, yaw, roll);
}
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/

55
samples/sample_c/module_sample/fc_subscription/test_fc_subscription.h Normal file
View File

@ -0,0 +1,55 @@
/**
********************************************************************
* @file test_fc_subscription.h
* @brief This is the header file for "test_fc_subscription.c", 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_FC_SUBSCRIPTION_H
#define TEST_FC_SUBSCRIPTION_H
/* Includes ------------------------------------------------------------------*/
#include "dji_typedef.h"
#include "dji_fc_subscription.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
T_DjiReturnCode DjiTest_FcSubscriptionStartService(void);
T_DjiReturnCode DjiTest_FcSubscriptionRunSample(void);
T_DjiReturnCode DjiTest_FcSubscriptionDataShowTrigger(void);
T_DjiReturnCode DjiTest_FcSubscriptionGetTotalSatelliteNumber(uint8_t *number);
#ifdef __cplusplus
}
#endif
#endif // TEST_FC_SUBSCRIPTION_H
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/