PSDK3.2.0精准定位、时间获取的问题
Completed当前使用PSDK3.2.0,环境RTOS,飞行器M300-RTK
1、在使用精准定位功能时,时间转换可以成功转换,但是调用DjiPositioning_GetPositionInformationSync后,positionInfo内数据都是0,飞行器在户外飞行,卫星41,RTK信号良好测试的时间是17:00,但是aircraftTime内的时间为8:47:,不知是否跟这个时间转换错误有关系呢?(代码见以下评论)
2、使用数据订阅时,订阅DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME项,在转换代码见下文,转换出来的时间不正确,且秒的计数已经超过了60,转换可能不正常。您那边能看下哪里有问题么?
-
bool PSDKRequestPosition(LocalTime_t *Local, uint8_t Count, PositionData_t *Pos){int32_t i = 0;T_DjiReturnCode djiStat;uint64_t ppsNewestTriggerTimeUs = 0;T_DjiPositioningEventInfo eventInfo[DJI_TEST_POSITIONING_EVENT_COUNT] = {0};T_DjiPositioningPositionInfo positionInfo[DJI_TEST_POSITIONING_EVENT_COUNT] = {0};T_DjiTimeSyncAircraftTime aircraftTime[DJI_TEST_POSITIONING_EVENT_COUNT] = {0};
for (i = 0; i < Count; ++i){eventInfo[i].eventSetIndex = s_eventIndex;eventInfo[i].targetPointIndex = i;
djiStat = DjiTimeSync_TransferToAircraftTime(Local[i].u64LocalTime, &aircraftTime[i]);if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS){USER_LOG_ERROR("transfer to aircraft time error: 0x%08llX.", djiStat);return false;}
eventInfo[i].eventTime = aircraftTime[i];}
djiStat = DjiPositioning_GetPositionInformationSync(Count, eventInfo, positionInfo);if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS){USER_LOG_ERROR("get position information error.");return false;}
for (i = 0; i < Count; ++i){Pos[i].u64SyncTime = aircraftTime[i].microsecond;Pos[i].u8Hour = aircraftTime[i].hour;Pos[i].u8Minute = aircraftTime[i].minute;Pos[i].u8Second = aircraftTime[i].second;Pos[i].f64Lon = positionInfo[i].targetPointPosition.longitude;Pos[i].f64Lat = positionInfo[i].targetPointPosition.latitude;Pos[i].f32Height = positionInfo[i].targetPointPosition.height;Pos[i].f32LonErr = positionInfo[i].targetPointPositionStandardDeviation.longitude;Pos[i].f32LatErr = positionInfo[i].targetPointPositionStandardDeviation.latitude;Pos[i].f32HeightErr = positionInfo[i].targetPointPositionStandardDeviation.height;Pos[i].f32ReqAirPitch = positionInfo[i].uavAttitude.pitch;Pos[i].f32ReqAirRoll = positionInfo[i].uavAttitude.roll;Pos[i].f32ReqAirYaw = positionInfo[i].uavAttitude.yaw;Pos[i].u8RTKStatus = positionInfo[i].positionSolutionProperty;}
USER_LOG_DEBUG("request position of target points success.");USER_LOG_DEBUG("detail position information:");for (i = 0; i < DJI_TEST_POSITIONING_EVENT_COUNT; ++i){USER_LOG_DEBUG("position solution property: %d.", positionInfo[i].positionSolutionProperty);USER_LOG_DEBUG("pitchAttitudeAngle: %d\trollAttitudeAngle: %d\tyawAttitudeAngle: %d",positionInfo[i].uavAttitude.pitch, positionInfo[i].uavAttitude.roll,positionInfo[i].uavAttitude.yaw);USER_LOG_DEBUG("northPositionOffset: %d\tearthPositionOffset: %d\tdownPositionOffset: %d",positionInfo[i].offsetBetweenMainAntennaAndTargetPoint.x,positionInfo[i].offsetBetweenMainAntennaAndTargetPoint.y,positionInfo[i].offsetBetweenMainAntennaAndTargetPoint.z);USER_LOG_DEBUG("longitude: %.8f\tlatitude: %.8f\theight: %.8f",positionInfo[i].targetPointPosition.longitude,positionInfo[i].targetPointPosition.latitude,positionInfo[i].targetPointPosition.height);USER_LOG_DEBUG("longStandardDeviation: %.8f\tlatStandardDeviation: %.8f\thgtStandardDeviation: %.8f",positionInfo[i].targetPointPositionStandardDeviation.longitude,positionInfo[i].targetPointPositionStandardDeviation.latitude,positionInfo[i].targetPointPositionStandardDeviation.height);}
s_eventIndex++;
if(positionInfo[0].positionSolutionProperty){return true;}else{return false;}} -
/**
********************************************************************
* @file test_time_sync.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 <fc_subscription/test_fc_subscription.h>
#include "test_time_sync.h"
#include "dji_time_sync.h"
#include "dji_logger.h"
#include "utils/util_misc.h"
#include "dji_platform.h"
#include "bsp_gpio.h"
#include "osal.h"/* Private constants ---------------------------------------------------------*/
#define DJI_TEST_TIME_SYNC_TASK_FREQ (1)
#define DJI_TEST_TIME_SYNC_TASK_STACK_SIZE (2048)/* Private types -------------------------------------------------------------*/
/* Private functions declaration ---------------------------------------------*/
static void *DjiTest_TimeSyncTask(void *arg);/* Private variables ---------------------------------------------------------*/
static T_DjiTestTimeSyncHandler s_timeSyncHandler;
static T_DjiTaskHandle s_timeSyncThread;
volatile static uint64_t s_ppsNewestTriggerLocalTimeMs = 0;
extern volatile uint64_t g_vu64SystemTick; //系统节拍1ms
/* Exported functions definition ---------------------------------------------*/
/**
* @brief Register handler function for initialising PPS pin configure and reporting the latest local time when PPS is
* triggered. This function have to be called before calling DjiTest_TimeSyncInit().
* @param timeSyncHandler: pointer to handler function for time synchronization.
* @return Execution result.
*/
T_DjiReturnCode DjiTest_TimeSyncRegHandler(T_DjiTestTimeSyncHandler *timeSyncHandler)
{
if (timeSyncHandler->PpsSignalResponseInit == NULL) {
USER_LOG_ERROR("reg time sync handler PpsSignalResponseInit error");
return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
}if (timeSyncHandler->GetNewestPpsTriggerLocalTimeUs == NULL) {
USER_LOG_ERROR("reg time sync handler GetNewestPpsTriggerLocalTimeUs error");
return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
}memcpy(&s_timeSyncHandler, timeSyncHandler, sizeof(T_DjiTestTimeSyncHandler));
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}T_DjiReturnCode DjiTest_TimeSyncStartService(void)
{
T_DjiReturnCode djiStat;
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();djiStat = DjiTimeSync_Init();
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("time synchronization module init error.");
return djiStat;
}if (s_timeSyncHandler.PpsSignalResponseInit == NULL) {
USER_LOG_ERROR("time sync handler PpsSignalResponseInit interface is NULL error");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}if (s_timeSyncHandler.GetNewestPpsTriggerLocalTimeUs == NULL) {
USER_LOG_ERROR("time sync handler GetNewestPpsTriggerLocalTimeUs interface is NULL error");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}// users must register getNewestPpsTriggerTime callback function
djiStat = DjiTimeSync_RegGetNewestPpsTriggerTimeCallback(s_timeSyncHandler.GetNewestPpsTriggerLocalTimeUs);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("register GetNewestPpsTriggerLocalTimeUsCallback error.");
return djiStat;
}if (osalHandler->TaskCreate("user_time_sync_task", DjiTest_TimeSyncTask,
DJI_TEST_TIME_SYNC_TASK_STACK_SIZE, NULL, &s_timeSyncThread) !=
DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("user time sync task create error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}djiStat = s_timeSyncHandler.PpsSignalResponseInit();
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("pps signal response init error");
return djiStat;
}return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}T_DjiReturnCode DjiTest_TimeSyncGetNewestPpsTriggerLocalTimeUs(uint64_t *localTimeUs)
{
if (s_timeSyncHandler.GetNewestPpsTriggerLocalTimeUs == NULL) {
USER_LOG_ERROR("GetNewestPpsTriggerLocalTimeUs null error.");
return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
}return s_timeSyncHandler.GetNewestPpsTriggerLocalTimeUs(localTimeUs);
}T_DjiReturnCode DjiTest_PpsSignalResponseInit(void)
{
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}void PSDKPpsTriggerCb(uint16_t GPIO_Pin)
{
T_DjiReturnCode psdkStat;
uint32_t timeMs = 0;
if(GPIO_Pin == J30J_PPS_Pin)
{
s_ppsNewestTriggerLocalTimeMs = g_vu64SystemTick;
// psdkStat = Osal_GetTimeMs(&timeMs);
// if (psdkStat == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
// {
// s_ppsNewestTriggerLocalTimeMs = timeMs;
// }
}
}T_DjiReturnCode DjiTest_GetNewestPpsTriggerLocalTimeUs(uint64_t *localTimeUs)
{
if (localTimeUs == NULL) {
USER_LOG_ERROR("input pointer is null.");
return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
}if (s_ppsNewestTriggerLocalTimeMs == 0) {
USER_LOG_WARN("pps have not been triggered.");
return DJI_ERROR_SYSTEM_MODULE_CODE_BUSY;
}*localTimeUs = (uint64_t) (s_ppsNewestTriggerLocalTimeMs * 1000);
//*localTimeUs = (uint64_t) (15);
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"
#endifstatic void *DjiTest_TimeSyncTask(void *arg)
{
T_DjiReturnCode djiStat;
uint32_t currentTimeMs = 0;
T_DjiTimeSyncAircraftTime aircraftTime = {0};
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
uint8_t totalSatelliteNumber = 0;USER_UTIL_UNUSED(arg);
while (1) {
osalHandler->TaskSleepMs(1000 / DJI_TEST_TIME_SYNC_TASK_FREQ);djiStat = DjiTest_FcSubscriptionGetTotalSatelliteNumber(&totalSatelliteNumber);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("get total satellite number error: 0x%08llX.", djiStat);
continue;
}djiStat = osalHandler->GetTimeMs(¤tTimeMs);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("get current time error: 0x%08llX.", djiStat);
continue;
}djiStat = DjiTimeSync_TransferToAircraftTime(currentTimeMs * 1000, &aircraftTime);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("transfer to aircraft time error: 0x%08llX.", djiStat);
continue;
}USER_LOG_DEBUG("current aircraft time is %d.%d.%d %d:%d %d %d.", aircraftTime.year, aircraftTime.month,
aircraftTime.day, aircraftTime.hour, aircraftTime.minute, aircraftTime.second,
aircraftTime.microsecond);
}
}#ifndef __CC_ARM
#pragma GCC diagnostic pop
#endif/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
-
1、在使用精准定位功能时,时间转换可以成功转换,但是调用DjiPositioning_GetPositionInformationSync后,positionInfo内数据都是0,飞行器在户外飞行,卫星41,RTK信号良好测试的时间是17:00,但是aircraftTime内的时间为8:47:,不知是否跟这个时间转换错误有关系呢?(代码见以下评论) -->转换的飞机时间是UTC时间,北京时间UTC+ 8,aircraftTime是将本地时间对应到飞机事件,再以这个时间作为事件来获取定位信息。建议先理清一下转换成飞机时间的本地时间。大致转换原理可以先参考 5.14 精准定位(positioning) 2、使用数据订阅时,订阅DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME项,在转换代码见下文,转换出来的时间不正确,且秒的计数已经超过了60,转换可能不正常。您那边能看下哪里有问题么? -->把DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME获取原始值打印出来看一下时间,看代码计算方式有误。格式:hhmmss,获取的是十进制数值。
Please sign in to leave a comment.
Comments
12 comments