消息订阅

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),
+                                                                     &timestamp);
+        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),
+                                                                     &timestamp);
+        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

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 <malloc.h>
+#include <stdlib.h>
+#include <time.h>
+
+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