From 6688d23e1382bf3318d2a6d3d01d143fd7ac387b Mon Sep 17 00:00:00 2001 From: tangchao0503 <735056338@qq.com> Date: Mon, 14 Aug 2023 11:40:53 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B6=88=E6=81=AF=E8=AE=A2=E9=98=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_subscription/test_fc_subscription.c | 208 ++++++++++++++++++ .../fc_subscription/test_fc_subscription.h | 14 ++ 2 files changed, 222 insertions(+) diff --git a/samples/sample_c/module_sample/fc_subscription/test_fc_subscription.c b/samples/sample_c/module_sample/fc_subscription/test_fc_subscription.c index 9a54dc1..7cddb82 100644 --- a/samples/sample_c/module_sample/fc_subscription/test_fc_subscription.c +++ b/samples/sample_c/module_sample/fc_subscription/test_fc_subscription.c @@ -41,6 +41,9 @@ static void *UserFcSubscription_Task(void *arg); static T_DjiReturnCode DjiTest_FcSubscriptionReceiveQuaternionCallback(const uint8_t *data, uint16_t dataSize, const T_DjiDataTimestamp *timestamp); +static char* uitoa(unsigned int n, char *s); +static int GMT_toLocalTime(unsigned long gmt_time,unsigned int* year,unsigned int* month,unsigned int* day,unsigned int* hour,unsigned int* minute,unsigned int* sec); + /* Private variables ---------------------------------------------------------*/ static T_DjiTaskHandle s_userFcSubscriptionThread; static bool s_userFcSubscriptionDataShow = false; @@ -48,6 +51,7 @@ static uint8_t s_totalSatelliteNumberUsed = 0; static uint32_t s_userFcSubscriptionDataCnt = 0; /* Exported functions definition ---------------------------------------------*/ +//要先订阅消息(DjiFcSubscription_SubscribeTopic),才能获取消息(DjiFcSubscription_GetLatestValueOfTopic) T_DjiReturnCode DjiTest_FcSubscriptionStartService(void) { T_DjiReturnCode djiStat; @@ -60,6 +64,7 @@ T_DjiReturnCode DjiTest_FcSubscriptionStartService(void) return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN; } + //订阅姿态四元数 djiStat = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION, DJI_DATA_SUBSCRIPTION_TOPIC_50_HZ, DjiTest_FcSubscriptionReceiveQuaternionCallback); if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { @@ -69,6 +74,7 @@ T_DjiReturnCode DjiTest_FcSubscriptionStartService(void) 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) { @@ -96,6 +102,21 @@ T_DjiReturnCode DjiTest_FcSubscriptionStartService(void) USER_LOG_DEBUG("Subscribe topic gps details success."); } + //订阅gps日期 + djiStat = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE, 1,NULL); + if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { + USER_LOG_ERROR("Subscribe topic QUATERNION error."); + return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN; + } + + //订阅gps时间 + djiStat = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME, 1,NULL); + if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { + USER_LOG_ERROR("Subscribe topic QUATERNION error."); + return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN; + } + + //创建用户任务 if (osalHandler->TaskCreate("user_subscription_task", UserFcSubscription_Task, FC_SUBSCRIPTION_TASK_STACK_SIZE, NULL, &s_userFcSubscriptionThread) != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { @@ -248,9 +269,14 @@ static void *UserFcSubscription_Task(void *arg) T_DjiFcSubscriptionGpsDetails gpsDetails = {0}; T_DjiOsalHandler *osalHandler = NULL; + T_DjiFcSubscriptionGpsDate gpsDate={0}; + T_DjiFcSubscriptionGpsTime gpsTime={0}; + USER_UTIL_UNUSED(arg); osalHandler = DjiPlatform_GetOsalHandler(); + int counter = 0; + bool isGetGpsDate_time = true; while (1) { osalHandler->TaskSleepMs(1000 / FC_SUBSCRIPTION_TASK_FREQ); @@ -295,9 +321,191 @@ static void *UserFcSubscription_Task(void *arg) s_totalSatelliteNumberUsed = gpsDetails.totalSatelliteNumberUsed; } + + + + + djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE, + (uint8_t *) &gpsDate, + sizeof(T_DjiFcSubscriptionGpsDate), + ×tamp); + if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { + USER_LOG_ERROR("get value of topic gps DATE error."); + isGetGpsDate_time = false; + } else { + USER_LOG_DEBUG("timestamp: millisecond %u microsecond %u.", timestamp.millisecond, + timestamp.microsecond); + USER_LOG_DEBUG("gps DATE: %d.", gpsDate); + }//PsdkLogger_UserLogInfo + + djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME, + (uint8_t *) &gpsTime, + sizeof(T_DjiFcSubscriptionGpsTime), + ×tamp); + if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { + USER_LOG_ERROR("get value of topic gps TIME error."); + isGetGpsDate_time = false; + } else { + USER_LOG_DEBUG("gps TIME: %d.", gpsTime); + }//PsdkLogger_UserLogInfo + + if(counter>1 &&counter<10 && isGetGpsDate_time)//避免dji的bug：第一次获取的gpsDate和gpsTime都为0 + { + printf("开始设置时间\n"); + system('date'); + + char *gpsDate_str = (char *)malloc(sizeof(char)*50); + char *gpsTime_str = (char *)malloc(sizeof(char)*20); + + + uitoa(gpsDate,gpsDate_str); + uitoa(gpsTime,gpsTime_str); + strcat(gpsDate_str, gpsTime_str); +// printf("pingjie----------------------------------------------:%s\n",gpsDate_str); + + + char *year = (char *)malloc(sizeof(char)*10); + char *mon = (char *)malloc(sizeof(char)*10); + char *mday = (char *)malloc(sizeof(char)*10); + char *hour = (char *)malloc(sizeof(char)*10); + char *min = (char *)malloc(sizeof(char)*10); + char *sec = (char *)malloc(sizeof(char)*10); + + strncpy(year, gpsDate_str, 4); + strncpy(mon, gpsDate_str+4, 2); + strncpy(mday, gpsDate_str+6, 2); + if(gpsTime >= 100000)//dji时间格式为hhmmss，但是当时间小于上午10点时，小时位（hh）只有1位：21923（2:19:23）； + { + strncpy(hour, gpsTime_str, 2); + strncpy(min, gpsTime_str+2, 2); + strncpy(sec, gpsTime_str+4, 2); + } + else + { + strncpy(hour, gpsTime_str, 1); + strncpy(min, gpsTime_str+1, 2); + strncpy(sec, gpsTime_str+3, 2); + } + + +// printf("year:%s\n",year); +// printf("mon:%s\n",mon); +// printf("mday:%s\n",mday); +// printf("hour:%s\n",hour); +// printf("min:%s\n",min); +// printf("sec:%s\n",sec); + + struct tm info; + info.tm_year = atoi(year) - 1900;; + info.tm_mon = atoi(mon) - 1; + info.tm_mday = atoi(mday); + info.tm_hour = atoi(hour) + 8; + info.tm_min = atoi(min); + info.tm_sec = atoi(sec); + info.tm_isdst = -1; + + int gmt_time = mktime(&info); + + LocalTimeInfo LocalTime; + + GMT_toLocalTime(gmt_time,&LocalTime.year,&LocalTime.month,&LocalTime.day,&LocalTime.hour,&LocalTime.minute,&LocalTime.second); +// printf("MyLocalTime=%d-%d-%d %d:%d:%d\n",LocalTime.year,LocalTime.month,LocalTime.day,LocalTime.hour,LocalTime.minute,LocalTime.second); + + char *timeParam = (char *)malloc(sizeof(char)*500); + sprintf(timeParam,"%d-%d-%d %d:%d:%d",LocalTime.year,LocalTime.month,LocalTime.day,LocalTime.hour,LocalTime.minute,LocalTime.second); +// printf("----------: %s",timeParam); + + char *command = (char *)malloc(sizeof(char)*100); + + strcat(command, "date -s \""); + strcat(command, timeParam); + strcat(command, "\""); + +// printf("command---:%s\n",command); + system(command); + } + counter++; + } } +char* uitoa(unsigned int n, char *s) +{ + int i, j; + i = 0; + char buf[20]; + memset(buf, 0, sizeof(buf)); + do{ + buf[i++] = n % 10 + '0';//取下一个数字 + } while ((n /= 10)>0);//删除该数字 + i -= 1; + for (j = 0; i >= 0; j++, i--)//生成的数字是逆序的，所以要逆序输出 + s[j] = buf[i]; + s[j] = '\0'; + return s; +} + +/*格林威治时间就是1970年01月01日00时00分00秒起至现在的总秒数*/ +/*通过格林威治时间，计算本地时间*/ +int GMT_toLocalTime(unsigned long gmt_time,unsigned int* year,unsigned int* month,unsigned int* day,unsigned int* hour,unsigned int* minute,unsigned int* sec) +{ + int TIME_ZONE = 8; + unsigned long gmtTime = gmt_time + TIME_ZONE * 60 * 60; + int leap_years = 0; + int month_days[] = {31, 28, 31,30,31, 30,31,31, 30,31,30,31}; + int i =0; + int days; + + *sec = (int)(gmtTime%60);//秒钟数 + gmtTime = gmtTime/60;//总共有多少分钟 + + *minute = (int)(gmtTime%60); + gmtTime = gmtTime/60;//总共有多少小时 + + *hour = (int)(gmtTime%24); + gmtTime = gmtTime/24;//总共有多少天 + + //去掉小时分钟秒钟后，转换成从公元元年开始到现在的天数 + //不包括今天 + gmtTime += 719162; +// 首先不考虑闰年计算年份和天数 +// 计算年份 + *year = (int)(gmtTime/365); +// days = （从公元元年开始到year的闰年个数 + 当前年份已经度过的天数）除以365后的余数 + days = (int)(gmtTime%365); + while(1) + { + //总共有多少个闰年，天数要相应的减去这些天数 + leap_years = (*year)/4 - (*year)/100; //计算普通闰年 + leap_years += (*year)/400; //加上世纪闰年 + if(days < leap_years) + { + days+=365; + (*year)--; + }else break; + } + days -= leap_years; + (*year)++; + days++; + //计算今年总共度过了多少秒 + if(((*year)%4 == 0 && (*year) % 100!=0) || (*year)%400==0) month_days[1] = 29;//今年是闰年,修改二月份为29天 + *month = 1; + for(i=0;i<12;i++) + { + if(days <= month_days[i]) + { + break; + } + else + { + days -=month_days[i]; + (*month)++; + } + } + *day =days; + return 0; +} + #ifndef __CC_ARM #pragma GCC diagnostic pop #endif diff --git a/samples/sample_c/module_sample/fc_subscription/test_fc_subscription.h b/samples/sample_c/module_sample/fc_subscription/test_fc_subscription.h index 3928100..3bfb334 100644 --- a/samples/sample_c/module_sample/fc_subscription/test_fc_subscription.h +++ b/samples/sample_c/module_sample/fc_subscription/test_fc_subscription.h @@ -31,6 +31,20 @@ #include "dji_typedef.h" #include "dji_fc_subscription.h" +#include +#include +#include + +typedef struct LocalTimeInformation +{ + unsigned int year; + unsigned int month; + unsigned int day; + unsigned int hour; + unsigned int minute; + unsigned int second; +}LocalTimeInfo; + #ifdef __cplusplus extern "C" { #endif