diff --git a/CMakeLists.txt b/CMakeLists.txt index ac694b3..e7bda5f 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ project(ydlidar_sdk C CXX) # version set(YDLIDAR_SDK_VERSION_MAJOR 1) set(YDLIDAR_SDK_VERSION_MINOR 2) -set(YDLIDAR_SDK_VERSION_PATCH 8) +set(YDLIDAR_SDK_VERSION_PATCH 9) set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH}) ########################################################## @@ -16,7 +16,7 @@ ELSE() SET(CMAKE_MRPT_WORD_SIZE 32) ENDIF() ################################################## -#c++ 11 +# Add c++11 Flag SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") ########################################################## diff --git a/core/base/thread.h b/core/base/thread.h index 5f6d7bb..b276b38 100644 --- a/core/base/thread.h +++ b/core/base/thread.h @@ -1,7 +1,6 @@ #pragma once #include "v8stdint.h" #include "timer.h" - #ifdef _WIN32 #include #include diff --git a/core/common/DriverInterface.h b/core/common/DriverInterface.h index 18cb400..4aef988 100644 --- a/core/common/DriverInterface.h +++ b/core/common/DriverInterface.h @@ -7,6 +7,7 @@ #include #include "ydlidar_protocol.h" #include "ydlidar_def.h" +#include "ydlidar_config.h" namespace ydlidar @@ -43,6 +44,8 @@ namespace ydlidar PropertyBuilderByName(bool, Debug, protected); // 扫描频率 PropertyBuilderByName(float, ScanFreq, protected); + // 采样率 + PropertyBuilderByName(float, SampleRate, protected); // 是否底板优先 PropertyBuilderByName(bool, Bottom, protected); // 是否已获取到设备信息 @@ -183,7 +186,9 @@ namespace ydlidar * static function * @return Version */ - virtual std::string getSDKVersion() = 0; + virtual std::string getSDKVersion() { + return YDLIDAR_SDK_VERSION_STR; + } /** * @brief Is the Lidar in the scan \n @@ -191,7 +196,9 @@ namespace ydlidar * @retval true scanning * @retval false non-scanning */ - virtual bool isscanning() const = 0; + virtual bool isscanning() const { + return m_isScanning; + } /** * @brief Is it connected to the lidar \n @@ -199,7 +206,9 @@ namespace ydlidar * @retval true connected * @retval false Non-connected */ - virtual bool isconnected() const = 0; + virtual bool isconnected() const { + return m_isConnected; + } /** * @brief Is there intensity \n @@ -222,7 +231,9 @@ namespace ydlidar * true support * false no support */ - virtual void setAutoReconnect(const bool &enable) = 0; + virtual void setAutoReconnect(const bool &enable) { + isAutoReconnect = enable; + } /** * @brief Get current scan update configuration. @@ -286,8 +297,13 @@ namespace ydlidar * @retval RESULT_FAILE failed * @note Just turn it on once */ - virtual result_t startScan(bool force = false, - uint32_t timeout = DEFAULT_TIMEOUT) = 0; + virtual result_t startScan( + bool force = false, + uint32_t timeout = DEFAULT_TIMEOUT) { + UNUSED(force); + UNUSED(timeout); + return RESULT_FAIL; + } /** * @brief turn off scanning \n @@ -295,7 +311,9 @@ namespace ydlidar * @retval RESULT_OK success * @retval RESULT_FAILE failed */ - virtual result_t stop() = 0; + virtual result_t stop() { + return RESULT_FAIL; + } /** * @brief Get a circle of laser data \n @@ -540,7 +558,11 @@ namespace ydlidar YDLIDAR_T15 = 200, /**< T15 LiDAR Model. */ - YDLIDAR_Tail, + YDLIDAR_TIA = 210, //TIA雷达 + YDLIDAR_TIA_H = 211, //TIA-H雷达 + YDLIDAR_TIA_X = 212, //TIA-X雷达 + + YDLIDAR_Tail = 255, }; enum YDLIDAR_RATE @@ -573,6 +595,7 @@ namespace ydlidar /// Parse Data thread Thread _thread; //线程对象 std::thread* m_thread = nullptr; //STD线程对象 + std::thread* m_thread2 = nullptr; //STD线程对象 /// command locker（不支持嵌套） Locker _cmd_lock; /// driver error locker（不支持嵌套） diff --git a/core/common/ydlidar_def.h b/core/common/ydlidar_def.h index f0a04ea..316bfa8 100644 --- a/core/common/ydlidar_def.h +++ b/core/common/ydlidar_def.h @@ -42,13 +42,14 @@ typedef enum { /** Lidar Type ID */ typedef enum { - TYPE_TOF = 0,/**< TG TX LiDAR.*/ - TYPE_TRIANGLE = 1,/**< G4. G6. G2 LiDAR.*/ - TYPE_TOF_NET = 2,/**< T15 LiDAR.*/ + TYPE_TOF = 0, //TG系列雷达 + TYPE_TRIANGLE = 1, //S2、S2 Pro、S4、G4、G6、G2、Tmini等三角协议雷达 + TYPE_TOF_NET = 2, //T系列雷达 TYPE_GS = 3, //GS系列雷达（目前只有GS2） TYPE_SCL = 4, //SCL雷达 TYPE_SDM = 5, //SDM15单点雷达 TYPE_SDM18 = 6, //SDM18单点雷达 + TYPE_TIA = 7, //TIA系列雷达 TYPE_Tail, } LidarTypeID; diff --git a/core/common/ydlidar_help.h b/core/common/ydlidar_help.h index 807b5a3..c0f6c2d 100644 --- a/core/common/ydlidar_help.h +++ b/core/common/ydlidar_help.h @@ -40,10 +40,10 @@ * * *********************************************************************/ #pragma once -#include "DriverInterface.h" #include #include #include +#include "DriverInterface.h" /** * @brief ydlidar @@ -59,6 +59,124 @@ using namespace base; */ namespace common { +//颜色定义 +#ifndef COLOR + #define COLOFF "\033[0m" ///关闭所有属性 + #define RED "\033[0;31m" ///"\033[显示方式;字体颜色;背景颜色m" + #define GREEN "\033[0;32m" + #define YELLOW "\033[0;33m" + #define BLUE "\033[0;34m" + #define PURPLE "\033[0;35m" +#endif + +//打印系统时间 +#define UNIX_PRINT_TIME \ + time_t currentTime = time(NULL); \ + struct tm *localTime = localtime(¤tTime); \ + printf("[%04d-%02d-%02d %02d:%02d:%02d]", \ + (1900 + localTime->tm_year), \ + (1 + localTime->tm_mon), \ + localTime->tm_mday, \ + localTime->tm_hour, \ + localTime->tm_min, \ + localTime->tm_sec); +//格式化字符串 +#define FORMAT_STDOUT \ + char buff[1024] = {0}; \ + va_list ap; \ + va_start(ap, fmt); \ + vsprintf(buff, fmt, ap); \ + va_end(ap); \ + printf(buff); \ + printf("\n"); + +//调试 +inline void debug(char* fmt, ...) +{ + printf(GREEN); //设置绿色 +#ifdef _WIN32 +#else + UNIX_PRINT_TIME +#endif + printf("[debug] "); + FORMAT_STDOUT + printf(COLOFF); //恢复默认颜色 + fflush(stdout); +} + +//常规 +inline void info(char* fmt, ...) +{ +#ifdef _WIN32 +#else + UNIX_PRINT_TIME +#endif + printf("[info] "); + FORMAT_STDOUT + fflush(stdout); +} + +//警告 +inline void warn(char* fmt, ...) +{ + printf(YELLOW); //设置黄色 +#ifdef _WIN32 +#else + UNIX_PRINT_TIME +#endif + printf("[warn] "); + FORMAT_STDOUT + printf(COLOFF); //恢复默认颜色 + fflush(stdout); +} + +//错误 +inline void error(char* fmt, ...) +{ + printf(RED); //设置红色 +#ifdef _WIN32 +#else + UNIX_PRINT_TIME +#endif + printf("[error] "); + FORMAT_STDOUT + printf(COLOFF); //恢复默认颜色 + fflush(stdout); +} + +//调试（16进制） +inline void debugh(const uint8_t *data, int size) +{ + if (!data || !size) + return; + printf(GREEN); //设置绿色 +#ifdef _WIN32 +#else + UNIX_PRINT_TIME +#endif + printf("[debug] "); + for (int i=0; i> 8); uint8_t Minjor = (uint8_t)(di.firmware_version & 0xff); + std::string sn; + for (int i = 0; i < SDK_SNLEN; i++) + sn += char(di.serialnum[i] + 48); //整型值转字符值 + // printf("%01X", di.serialnum[i] & 0xff); - printf("[YDLIDAR] %s device info\n" + info("%s device info\n" "Firmware version: %u.%u\n" "Hardware version: %u\n" "Model: %s\n" - "Serial: ", + "Serial: %s", EPT_Module == platformType ? "Module" : "Baseplate", Major, Minjor, di.hardware_version, - lidarModelToString(di.model).c_str()); - for (int i = 0; i < SDK_SNLEN; i++) - printf("%01X", di.serialnum[i] & 0xff); - printf("\n"); - fflush(stdout); + lidarModelToString(di.model).c_str(), + sn.c_str()); return true; } @@ -1036,91 +1160,34 @@ inline std::vector split(const std::string &s, char delim) { * @param protocol LiDAR Protocol Byte information * @return true if it is V1, otherwise false */ -inline bool isV1Protocol(uint8_t protocol) { +inline bool isV1Protocol(uint8_t protocol) +{ if (protocol == Protocol_V1) { return true; } - return false; } -//以16进制打印数据 -inline void printHex(const uint8_t *data, int size) -{ - if (!data || !size) - return; - for (int i=0; itm_year), \ - (1 + localTime->tm_mon), \ - localTime->tm_mday, \ - localTime->tm_hour, \ - localTime->tm_min, \ - localTime->tm_sec); -//格式化字符串 -#define FORMAT_STDOUT \ - char buff[1024] = {0}; \ - va_list ap; \ - va_start(ap, fmt); \ - vsprintf(buff, fmt, ap); \ - va_end(ap); \ - printf(buff); \ - printf("\n"); - -//调试 -inline void debug(char* fmt, ...) -{ -#ifdef _WIN32 -#else - UNIX_PRINT_TIME -#endif - printf("[debug] "); - FORMAT_STDOUT - fflush(stdout); -} - -//常规 -inline void info(char* fmt, ...) +//获取数据值（小端序） +inline uint32_t getLittleValue(const uint8_t *data, int size) { -#ifdef _WIN32 -#else - UNIX_PRINT_TIME -#endif - printf("[info] "); - FORMAT_STDOUT - fflush(stdout); + uint32_t v = 0; + if (!data || !size) + return v; + for (int i=0; i +#include +#include +#include +#include +#include +#include + +#ifdef ENABLE_LOCALES +#include +#endif + +#if defined(_MSC_VER) +#pragma warning (pop) +#endif +#ifdef __GNUC__ +#pragma GCC visibility pop +#endif + +#include "cJSON.h" + +/* define our own boolean type */ +#ifdef true +#undef true +#endif +#define true ((cJSON_bool)1) + +#ifdef false +#undef false +#endif +#define false ((cJSON_bool)0) + +/* define isnan and isinf for ANSI C, if in C99 or above, isnan and isinf has been defined in math.h */ +#ifndef isinf +#define isinf(d) (isnan((d - d)) && !isnan(d)) +#endif +#ifndef isnan +#define isnan(d) (d != d) +#endif + +#ifndef NAN +#ifdef _WIN32 +#define NAN sqrt(-1.0) +#else +#define NAN 0.0/0.0 +#endif +#endif + +typedef struct { + const unsigned char *json; + size_t position; +} error; +static error global_error = { NULL, 0 }; + +CJSON_PUBLIC(const char *) cJSON_GetErrorPtr(void) +{ + return (const char*) (global_error.json + global_error.position); +} + +CJSON_PUBLIC(char *) cJSON_GetStringValue(const cJSON * const item) +{ + if (!cJSON_IsString(item)) + { + return NULL; + } + + return item->valuestring; +} + +CJSON_PUBLIC(double) cJSON_GetNumberValue(const cJSON * const item) +{ + if (!cJSON_IsNumber(item)) + { + return (double) NAN; + } + + return item->valuedouble; +} + +/* This is a safeguard to prevent copy-pasters from using incompatible C and header files */ +#if (CJSON_VERSION_MAJOR != 1) || (CJSON_VERSION_MINOR != 7) || (CJSON_VERSION_PATCH != 15) + #error cJSON.h and cJSON.c have different versions. Make sure that both have the same. +#endif + +CJSON_PUBLIC(const char*) cJSON_Version(void) +{ + static char version[15]; + sprintf(version, "%i.%i.%i", CJSON_VERSION_MAJOR, CJSON_VERSION_MINOR, CJSON_VERSION_PATCH); + + return version; +} + +/* Case insensitive string comparison, doesn't consider two NULL pointers equal though */ +static int case_insensitive_strcmp(const unsigned char *string1, const unsigned char *string2) +{ + if ((string1 == NULL) || (string2 == NULL)) + { + return 1; + } + + if (string1 == string2) + { + return 0; + } + + for(; tolower(*string1) == tolower(*string2); (void)string1++, string2++) + { + if (*string1 == '\0') + { + return 0; + } + } + + return tolower(*string1) - tolower(*string2); +} + +typedef struct internal_hooks +{ + void *(CJSON_CDECL *allocate)(size_t size); + void (CJSON_CDECL *deallocate)(void *pointer); + void *(CJSON_CDECL *reallocate)(void *pointer, size_t size); +} internal_hooks; + +#if defined(_MSC_VER) +/* work around MSVC error C2322: '...' address of dllimport '...' is not static */ +static void * CJSON_CDECL internal_malloc(size_t size) +{ + return malloc(size); +} +static void CJSON_CDECL internal_free(void *pointer) +{ + free(pointer); +} +static void * CJSON_CDECL internal_realloc(void *pointer, size_t size) +{ + return realloc(pointer, size); +} +#else +#define internal_malloc malloc +#define internal_free free +#define internal_realloc realloc +#endif + +/* strlen of character literals resolved at compile time */ +#define static_strlen(string_literal) (sizeof(string_literal) - sizeof("")) + +static internal_hooks global_hooks = { internal_malloc, internal_free, internal_realloc }; + +static unsigned char* cJSON_strdup(const unsigned char* string, const internal_hooks * const hooks) +{ + size_t length = 0; + unsigned char *copy = NULL; + + if (string == NULL) + { + return NULL; + } + + length = strlen((const char*)string) + sizeof(""); + copy = (unsigned char*)hooks->allocate(length); + if (copy == NULL) + { + return NULL; + } + memcpy(copy, string, length); + + return copy; +} + +CJSON_PUBLIC(void) cJSON_InitHooks(cJSON_Hooks* hooks) +{ + if (hooks == NULL) + { + /* Reset hooks */ + global_hooks.allocate = malloc; + global_hooks.deallocate = free; + global_hooks.reallocate = realloc; + return; + } + + global_hooks.allocate = malloc; + if (hooks->malloc_fn != NULL) + { + global_hooks.allocate = hooks->malloc_fn; + } + + global_hooks.deallocate = free; + if (hooks->free_fn != NULL) + { + global_hooks.deallocate = hooks->free_fn; + } + + /* use realloc only if both free and malloc are used */ + global_hooks.reallocate = NULL; + if ((global_hooks.allocate == malloc) && (global_hooks.deallocate == free)) + { + global_hooks.reallocate = realloc; + } +} + +/* Internal constructor. */ +static cJSON *cJSON_New_Item(const internal_hooks * const hooks) +{ + cJSON* node = (cJSON*)hooks->allocate(sizeof(cJSON)); + if (node) + { + memset(node, '\0', sizeof(cJSON)); + } + + return node; +} + +/* Delete a cJSON structure. */ +CJSON_PUBLIC(void) cJSON_Delete(cJSON *item) +{ + cJSON *next = NULL; + while (item != NULL) + { + next = item->next; + if (!(item->type & cJSON_IsReference) && (item->child != NULL)) + { + cJSON_Delete(item->child); + } + if (!(item->type & cJSON_IsReference) && (item->valuestring != NULL)) + { + global_hooks.deallocate(item->valuestring); + } + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + global_hooks.deallocate(item->string); + } + global_hooks.deallocate(item); + item = next; + } +} + +/* get the decimal point character of the current locale */ +static unsigned char get_decimal_point(void) +{ +#ifdef ENABLE_LOCALES + struct lconv *lconv = localeconv(); + return (unsigned char) lconv->decimal_point[0]; +#else + return '.'; +#endif +} + +typedef struct +{ + const unsigned char *content; + size_t length; + size_t offset; + size_t depth; /* How deeply nested (in arrays/objects) is the input at the current offset. */ + internal_hooks hooks; +} parse_buffer; + +/* check if the given size is left to read in a given parse buffer (starting with 1) */ +#define can_read(buffer, size) ((buffer != NULL) && (((buffer)->offset + size) <= (buffer)->length)) +/* check if the buffer can be accessed at the given index (starting with 0) */ +#define can_access_at_index(buffer, index) ((buffer != NULL) && (((buffer)->offset + index) < (buffer)->length)) +#define cannot_access_at_index(buffer, index) (!can_access_at_index(buffer, index)) +/* get a pointer to the buffer at the position */ +#define buffer_at_offset(buffer) ((buffer)->content + (buffer)->offset) + +/* Parse the input text to generate a number, and populate the result into item. */ +static cJSON_bool parse_number(cJSON * const item, parse_buffer * const input_buffer) +{ + double number = 0; + unsigned char *after_end = NULL; + unsigned char number_c_string[64]; + unsigned char decimal_point = get_decimal_point(); + size_t i = 0; + + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; + } + + /* copy the number into a temporary buffer and replace '.' with the decimal point + * of the current locale (for strtod) + * This also takes care of '\0' not necessarily being available for marking the end of the input */ + for (i = 0; (i < (sizeof(number_c_string) - 1)) && can_access_at_index(input_buffer, i); i++) + { + switch (buffer_at_offset(input_buffer)[i]) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '+': + case '-': + case 'e': + case 'E': + number_c_string[i] = buffer_at_offset(input_buffer)[i]; + break; + + case '.': + number_c_string[i] = decimal_point; + break; + + default: + goto loop_end; + } + } +loop_end: + number_c_string[i] = '\0'; + + number = strtod((const char*)number_c_string, (char**)&after_end); + if (number_c_string == after_end) + { + return false; /* parse_error */ + } + + item->valuedouble = number; + + /* use saturation in case of overflow */ + if (number >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)number; + } + + item->type = cJSON_Number; + + input_buffer->offset += (size_t)(after_end - number_c_string); + return true; +} + +/* don't ask me, but the original cJSON_SetNumberValue returns an integer or double */ +CJSON_PUBLIC(double) cJSON_SetNumberHelper(cJSON *object, double number) +{ + if (number >= INT_MAX) + { + object->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + object->valueint = INT_MIN; + } + else + { + object->valueint = (int)number; + } + + return object->valuedouble = number; +} + +CJSON_PUBLIC(char*) cJSON_SetValuestring(cJSON *object, const char *valuestring) +{ + char *copy = NULL; + /* if object's type is not cJSON_String or is cJSON_IsReference, it should not set valuestring */ + if (!(object->type & cJSON_String) || (object->type & cJSON_IsReference)) + { + return NULL; + } + if (strlen(valuestring) <= strlen(object->valuestring)) + { + strcpy(object->valuestring, valuestring); + return object->valuestring; + } + copy = (char*) cJSON_strdup((const unsigned char*)valuestring, &global_hooks); + if (copy == NULL) + { + return NULL; + } + if (object->valuestring != NULL) + { + cJSON_free(object->valuestring); + } + object->valuestring = copy; + + return copy; +} + +typedef struct +{ + unsigned char *buffer; + size_t length; + size_t offset; + size_t depth; /* current nesting depth (for formatted printing) */ + cJSON_bool noalloc; + cJSON_bool format; /* is this print a formatted print */ + internal_hooks hooks; +} printbuffer; + +/* realloc printbuffer if necessary to have at least "needed" bytes more */ +static unsigned char* ensure(printbuffer * const p, size_t needed) +{ + unsigned char *newbuffer = NULL; + size_t newsize = 0; + + if ((p == NULL) || (p->buffer == NULL)) + { + return NULL; + } + + if ((p->length > 0) && (p->offset >= p->length)) + { + /* make sure that offset is valid */ + return NULL; + } + + if (needed > INT_MAX) + { + /* sizes bigger than INT_MAX are currently not supported */ + return NULL; + } + + needed += p->offset + 1; + if (needed <= p->length) + { + return p->buffer + p->offset; + } + + if (p->noalloc) { + return NULL; + } + + /* calculate new buffer size */ + if (needed > (INT_MAX / 2)) + { + /* overflow of int, use INT_MAX if possible */ + if (needed <= INT_MAX) + { + newsize = INT_MAX; + } + else + { + return NULL; + } + } + else + { + newsize = needed * 2; + } + + if (p->hooks.reallocate != NULL) + { + /* reallocate with realloc if available */ + newbuffer = (unsigned char*)p->hooks.reallocate(p->buffer, newsize); + if (newbuffer == NULL) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + } + else + { + /* otherwise reallocate manually */ + newbuffer = (unsigned char*)p->hooks.allocate(newsize); + if (!newbuffer) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + + memcpy(newbuffer, p->buffer, p->offset + 1); + p->hooks.deallocate(p->buffer); + } + p->length = newsize; + p->buffer = newbuffer; + + return newbuffer + p->offset; +} + +/* calculate the new length of the string in a printbuffer and update the offset */ +static void update_offset(printbuffer * const buffer) +{ + const unsigned char *buffer_pointer = NULL; + if ((buffer == NULL) || (buffer->buffer == NULL)) + { + return; + } + buffer_pointer = buffer->buffer + buffer->offset; + + buffer->offset += strlen((const char*)buffer_pointer); +} + +/* securely comparison of floating-point variables */ +static cJSON_bool compare_double(double a, double b) +{ + double maxVal = fabs(a) > fabs(b) ? fabs(a) : fabs(b); + return (fabs(a - b) <= maxVal * DBL_EPSILON); +} + +/* Render the number nicely from the given item into a string. */ +static cJSON_bool print_number(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + double d = item->valuedouble; + int length = 0; + size_t i = 0; + unsigned char number_buffer[26] = {0}; /* temporary buffer to print the number into */ + unsigned char decimal_point = get_decimal_point(); + double test = 0.0; + + if (output_buffer == NULL) + { + return false; + } + + /* This checks for NaN and Infinity */ + if (isnan(d) || isinf(d)) + { + length = sprintf((char*)number_buffer, "null"); + } + else if(d == (double)item->valueint) + { + length = sprintf((char*)number_buffer, "%d", item->valueint); + } + else + { + /* Try 15 decimal places of precision to avoid nonsignificant nonzero digits */ + length = sprintf((char*)number_buffer, "%1.15g", d); + + /* Check whether the original double can be recovered */ + if ((sscanf((char*)number_buffer, "%lg", &test) != 1) || !compare_double((double)test, d)) + { + /* If not, print with 17 decimal places of precision */ + length = sprintf((char*)number_buffer, "%1.17g", d); + } + } + + /* sprintf failed or buffer overrun occurred */ + if ((length < 0) || (length > (int)(sizeof(number_buffer) - 1))) + { + return false; + } + + /* reserve appropriate space in the output */ + output_pointer = ensure(output_buffer, (size_t)length + sizeof("")); + if (output_pointer == NULL) + { + return false; + } + + /* copy the printed number to the output and replace locale + * dependent decimal point with '.' */ + for (i = 0; i < ((size_t)length); i++) + { + if (number_buffer[i] == decimal_point) + { + output_pointer[i] = '.'; + continue; + } + + output_pointer[i] = number_buffer[i]; + } + output_pointer[i] = '\0'; + + output_buffer->offset += (size_t)length; + + return true; +} + +/* parse 4 digit hexadecimal number */ +static unsigned parse_hex4(const unsigned char * const input) +{ + unsigned int h = 0; + size_t i = 0; + + for (i = 0; i < 4; i++) + { + /* parse digit */ + if ((input[i] >= '0') && (input[i] <= '9')) + { + h += (unsigned int) input[i] - '0'; + } + else if ((input[i] >= 'A') && (input[i] <= 'F')) + { + h += (unsigned int) 10 + input[i] - 'A'; + } + else if ((input[i] >= 'a') && (input[i] <= 'f')) + { + h += (unsigned int) 10 + input[i] - 'a'; + } + else /* invalid */ + { + return 0; + } + + if (i < 3) + { + /* shift left to make place for the next nibble */ + h = h << 4; + } + } + + return h; +} + +/* converts a UTF-16 literal to UTF-8 + * A literal can be one or two sequences of the form \uXXXX */ +static unsigned char utf16_literal_to_utf8(const unsigned char * const input_pointer, const unsigned char * const input_end, unsigned char **output_pointer) +{ + long unsigned int codepoint = 0; + unsigned int first_code = 0; + const unsigned char *first_sequence = input_pointer; + unsigned char utf8_length = 0; + unsigned char utf8_position = 0; + unsigned char sequence_length = 0; + unsigned char first_byte_mark = 0; + + if ((input_end - first_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + /* get the first utf16 sequence */ + first_code = parse_hex4(first_sequence + 2); + + /* check that the code is valid */ + if (((first_code >= 0xDC00) && (first_code <= 0xDFFF))) + { + goto fail; + } + + /* UTF16 surrogate pair */ + if ((first_code >= 0xD800) && (first_code <= 0xDBFF)) + { + const unsigned char *second_sequence = first_sequence + 6; + unsigned int second_code = 0; + sequence_length = 12; /* \uXXXX\uXXXX */ + + if ((input_end - second_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + if ((second_sequence[0] != '\\') || (second_sequence[1] != 'u')) + { + /* missing second half of the surrogate pair */ + goto fail; + } + + /* get the second utf16 sequence */ + second_code = parse_hex4(second_sequence + 2); + /* check that the code is valid */ + if ((second_code < 0xDC00) || (second_code > 0xDFFF)) + { + /* invalid second half of the surrogate pair */ + goto fail; + } + + + /* calculate the unicode codepoint from the surrogate pair */ + codepoint = 0x10000 + (((first_code & 0x3FF) << 10) | (second_code & 0x3FF)); + } + else + { + sequence_length = 6; /* \uXXXX */ + codepoint = first_code; + } + + /* encode as UTF-8 + * takes at maximum 4 bytes to encode: + * 11110xxx 10xxxxxx 10xxxxxx 10xxxxxx */ + if (codepoint < 0x80) + { + /* normal ascii, encoding 0xxxxxxx */ + utf8_length = 1; + } + else if (codepoint < 0x800) + { + /* two bytes, encoding 110xxxxx 10xxxxxx */ + utf8_length = 2; + first_byte_mark = 0xC0; /* 11000000 */ + } + else if (codepoint < 0x10000) + { + /* three bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx */ + utf8_length = 3; + first_byte_mark = 0xE0; /* 11100000 */ + } + else if (codepoint <= 0x10FFFF) + { + /* four bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx 10xxxxxx */ + utf8_length = 4; + first_byte_mark = 0xF0; /* 11110000 */ + } + else + { + /* invalid unicode codepoint */ + goto fail; + } + + /* encode as utf8 */ + for (utf8_position = (unsigned char)(utf8_length - 1); utf8_position > 0; utf8_position--) + { + /* 10xxxxxx */ + (*output_pointer)[utf8_position] = (unsigned char)((codepoint | 0x80) & 0xBF); + codepoint >>= 6; + } + /* encode first byte */ + if (utf8_length > 1) + { + (*output_pointer)[0] = (unsigned char)((codepoint | first_byte_mark) & 0xFF); + } + else + { + (*output_pointer)[0] = (unsigned char)(codepoint & 0x7F); + } + + *output_pointer += utf8_length; + + return sequence_length; + +fail: + return 0; +} + +/* Parse the input text into an unescaped cinput, and populate item. */ +static cJSON_bool parse_string(cJSON * const item, parse_buffer * const input_buffer) +{ + const unsigned char *input_pointer = buffer_at_offset(input_buffer) + 1; + const unsigned char *input_end = buffer_at_offset(input_buffer) + 1; + unsigned char *output_pointer = NULL; + unsigned char *output = NULL; + + /* not a string */ + if (buffer_at_offset(input_buffer)[0] != '\"') + { + goto fail; + } + + { + /* calculate approximate size of the output (overestimate) */ + size_t allocation_length = 0; + size_t skipped_bytes = 0; + while (((size_t)(input_end - input_buffer->content) < input_buffer->length) && (*input_end != '\"')) + { + /* is escape sequence */ + if (input_end[0] == '\\') + { + if ((size_t)(input_end + 1 - input_buffer->content) >= input_buffer->length) + { + /* prevent buffer overflow when last input character is a backslash */ + goto fail; + } + skipped_bytes++; + input_end++; + } + input_end++; + } + if (((size_t)(input_end - input_buffer->content) >= input_buffer->length) || (*input_end != '\"')) + { + goto fail; /* string ended unexpectedly */ + } + + /* This is at most how much we need for the output */ + allocation_length = (size_t) (input_end - buffer_at_offset(input_buffer)) - skipped_bytes; + output = (unsigned char*)input_buffer->hooks.allocate(allocation_length + sizeof("")); + if (output == NULL) + { + goto fail; /* allocation failure */ + } + } + + output_pointer = output; + /* loop through the string literal */ + while (input_pointer < input_end) + { + if (*input_pointer != '\\') + { + *output_pointer++ = *input_pointer++; + } + /* escape sequence */ + else + { + unsigned char sequence_length = 2; + if ((input_end - input_pointer) < 1) + { + goto fail; + } + + switch (input_pointer[1]) + { + case 'b': + *output_pointer++ = '\b'; + break; + case 'f': + *output_pointer++ = '\f'; + break; + case 'n': + *output_pointer++ = '\n'; + break; + case 'r': + *output_pointer++ = '\r'; + break; + case 't': + *output_pointer++ = '\t'; + break; + case '\"': + case '\\': + case '/': + *output_pointer++ = input_pointer[1]; + break; + + /* UTF-16 literal */ + case 'u': + sequence_length = utf16_literal_to_utf8(input_pointer, input_end, &output_pointer); + if (sequence_length == 0) + { + /* failed to convert UTF16-literal to UTF-8 */ + goto fail; + } + break; + + default: + goto fail; + } + input_pointer += sequence_length; + } + } + + /* zero terminate the output */ + *output_pointer = '\0'; + + item->type = cJSON_String; + item->valuestring = (char*)output; + + input_buffer->offset = (size_t) (input_end - input_buffer->content); + input_buffer->offset++; + + return true; + +fail: + if (output != NULL) + { + input_buffer->hooks.deallocate(output); + } + + if (input_pointer != NULL) + { + input_buffer->offset = (size_t)(input_pointer - input_buffer->content); + } + + return false; +} + +/* Render the cstring provided to an escaped version that can be printed. */ +static cJSON_bool print_string_ptr(const unsigned char * const input, printbuffer * const output_buffer) +{ + const unsigned char *input_pointer = NULL; + unsigned char *output = NULL; + unsigned char *output_pointer = NULL; + size_t output_length = 0; + /* numbers of additional characters needed for escaping */ + size_t escape_characters = 0; + + if (output_buffer == NULL) + { + return false; + } + + /* empty string */ + if (input == NULL) + { + output = ensure(output_buffer, sizeof("\"\"")); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "\"\""); + + return true; + } + + /* set "flag" to 1 if something needs to be escaped */ + for (input_pointer = input; *input_pointer; input_pointer++) + { + switch (*input_pointer) + { + case '\"': + case '\\': + case '\b': + case '\f': + case '\n': + case '\r': + case '\t': + /* one character escape sequence */ + escape_characters++; + break; + default: + if (*input_pointer < 32) + { + /* UTF-16 escape sequence uXXXX */ + escape_characters += 5; + } + break; + } + } + output_length = (size_t)(input_pointer - input) + escape_characters; + + output = ensure(output_buffer, output_length + sizeof("\"\"")); + if (output == NULL) + { + return false; + } + + /* no characters have to be escaped */ + if (escape_characters == 0) + { + output[0] = '\"'; + memcpy(output + 1, input, output_length); + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; + } + + output[0] = '\"'; + output_pointer = output + 1; + /* copy the string */ + for (input_pointer = input; *input_pointer != '\0'; (void)input_pointer++, output_pointer++) + { + if ((*input_pointer > 31) && (*input_pointer != '\"') && (*input_pointer != '\\')) + { + /* normal character, copy */ + *output_pointer = *input_pointer; + } + else + { + /* character needs to be escaped */ + *output_pointer++ = '\\'; + switch (*input_pointer) + { + case '\\': + *output_pointer = '\\'; + break; + case '\"': + *output_pointer = '\"'; + break; + case '\b': + *output_pointer = 'b'; + break; + case '\f': + *output_pointer = 'f'; + break; + case '\n': + *output_pointer = 'n'; + break; + case '\r': + *output_pointer = 'r'; + break; + case '\t': + *output_pointer = 't'; + break; + default: + /* escape and print as unicode codepoint */ + sprintf((char*)output_pointer, "u%04x", *input_pointer); + output_pointer += 4; + break; + } + } + } + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; +} + +/* Invoke print_string_ptr (which is useful) on an item. */ +static cJSON_bool print_string(const cJSON * const item, printbuffer * const p) +{ + return print_string_ptr((unsigned char*)item->valuestring, p); +} + +/* Predeclare these prototypes. */ +static cJSON_bool parse_value(cJSON * const item, parse_buffer * const input_buffer); +static cJSON_bool print_value(const cJSON * const item, printbuffer * const output_buffer); +static cJSON_bool parse_array(cJSON * const item, parse_buffer * const input_buffer); +static cJSON_bool print_array(const cJSON * const item, printbuffer * const output_buffer); +static cJSON_bool parse_object(cJSON * const item, parse_buffer * const input_buffer); +static cJSON_bool print_object(const cJSON * const item, printbuffer * const output_buffer); + +/* Utility to jump whitespace and cr/lf */ +static parse_buffer *buffer_skip_whitespace(parse_buffer * const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL)) + { + return NULL; + } + + if (cannot_access_at_index(buffer, 0)) + { + return buffer; + } + + while (can_access_at_index(buffer, 0) && (buffer_at_offset(buffer)[0] <= 32)) + { + buffer->offset++; + } + + if (buffer->offset == buffer->length) + { + buffer->offset--; + } + + return buffer; +} + +/* skip the UTF-8 BOM (byte order mark) if it is at the beginning of a buffer */ +static parse_buffer *skip_utf8_bom(parse_buffer * const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL) || (buffer->offset != 0)) + { + return NULL; + } + + if (can_access_at_index(buffer, 4) && (strncmp((const char*)buffer_at_offset(buffer), "\xEF\xBB\xBF", 3) == 0)) + { + buffer->offset += 3; + } + + return buffer; +} + +CJSON_PUBLIC(cJSON *) cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + size_t buffer_length; + + if (NULL == value) + { + return NULL; + } + + /* Adding null character size due to require_null_terminated. */ + buffer_length = strlen(value) + sizeof(""); + + return cJSON_ParseWithLengthOpts(value, buffer_length, return_parse_end, require_null_terminated); +} + +/* Parse an object - create a new root, and populate. */ +CJSON_PUBLIC(cJSON *) cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + parse_buffer buffer = { 0, 0, 0, 0, { 0, 0, 0 } }; + cJSON *item = NULL; + + /* reset error position */ + global_error.json = NULL; + global_error.position = 0; + + if (value == NULL || 0 == buffer_length) + { + goto fail; + } + + buffer.content = (const unsigned char*)value; + buffer.length = buffer_length; + buffer.offset = 0; + buffer.hooks = global_hooks; + + item = cJSON_New_Item(&global_hooks); + if (item == NULL) /* memory fail */ + { + goto fail; + } + + if (!parse_value(item, buffer_skip_whitespace(skip_utf8_bom(&buffer)))) + { + /* parse failure. ep is set. */ + goto fail; + } + + /* if we require null-terminated JSON without appended garbage, skip and then check for a null terminator */ + if (require_null_terminated) + { + buffer_skip_whitespace(&buffer); + if ((buffer.offset >= buffer.length) || buffer_at_offset(&buffer)[0] != '\0') + { + goto fail; + } + } + if (return_parse_end) + { + *return_parse_end = (const char*)buffer_at_offset(&buffer); + } + + return item; + +fail: + if (item != NULL) + { + cJSON_Delete(item); + } + + if (value != NULL) + { + error local_error; + local_error.json = (const unsigned char*)value; + local_error.position = 0; + + if (buffer.offset < buffer.length) + { + local_error.position = buffer.offset; + } + else if (buffer.length > 0) + { + local_error.position = buffer.length - 1; + } + + if (return_parse_end != NULL) + { + *return_parse_end = (const char*)local_error.json + local_error.position; + } + + global_error = local_error; + } + + return NULL; +} + +/* Default options for cJSON_Parse */ +CJSON_PUBLIC(cJSON *) cJSON_Parse(const char *value) +{ + return cJSON_ParseWithOpts(value, 0, 0); +} + +CJSON_PUBLIC(cJSON *) cJSON_ParseWithLength(const char *value, size_t buffer_length) +{ + return cJSON_ParseWithLengthOpts(value, buffer_length, 0, 0); +} + +#define cjson_min(a, b) (((a) < (b)) ? (a) : (b)) + +static unsigned char *print(const cJSON * const item, cJSON_bool format, const internal_hooks * const hooks) +{ + static const size_t default_buffer_size = 256; + printbuffer buffer[1]; + unsigned char *printed = NULL; + + memset(buffer, 0, sizeof(buffer)); + + /* create buffer */ + buffer->buffer = (unsigned char*) hooks->allocate(default_buffer_size); + buffer->length = default_buffer_size; + buffer->format = format; + buffer->hooks = *hooks; + if (buffer->buffer == NULL) + { + goto fail; + } + + /* print the value */ + if (!print_value(item, buffer)) + { + goto fail; + } + update_offset(buffer); + + /* check if reallocate is available */ + if (hooks->reallocate != NULL) + { + printed = (unsigned char*) hooks->reallocate(buffer->buffer, buffer->offset + 1); + if (printed == NULL) { + goto fail; + } + buffer->buffer = NULL; + } + else /* otherwise copy the JSON over to a new buffer */ + { + printed = (unsigned char*) hooks->allocate(buffer->offset + 1); + if (printed == NULL) + { + goto fail; + } + memcpy(printed, buffer->buffer, cjson_min(buffer->length, buffer->offset + 1)); + printed[buffer->offset] = '\0'; /* just to be sure */ + + /* free the buffer */ + hooks->deallocate(buffer->buffer); + } + + return printed; + +fail: + if (buffer->buffer != NULL) + { + hooks->deallocate(buffer->buffer); + } + + if (printed != NULL) + { + hooks->deallocate(printed); + } + + return NULL; +} + +/* Render a cJSON item/entity/structure to text. */ +CJSON_PUBLIC(char *) cJSON_Print(const cJSON *item) +{ + return (char*)print(item, true, &global_hooks); +} + +CJSON_PUBLIC(char *) cJSON_PrintUnformatted(const cJSON *item) +{ + return (char*)print(item, false, &global_hooks); +} + +CJSON_PUBLIC(char *) cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt) +{ + printbuffer p = { 0, 0, 0, 0, 0, 0, { 0, 0, 0 } }; + + if (prebuffer < 0) + { + return NULL; + } + + p.buffer = (unsigned char*)global_hooks.allocate((size_t)prebuffer); + if (!p.buffer) + { + return NULL; + } + + p.length = (size_t)prebuffer; + p.offset = 0; + p.noalloc = false; + p.format = fmt; + p.hooks = global_hooks; + + if (!print_value(item, &p)) + { + global_hooks.deallocate(p.buffer); + return NULL; + } + + return (char*)p.buffer; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format) +{ + printbuffer p = { 0, 0, 0, 0, 0, 0, { 0, 0, 0 } }; + + if ((length < 0) || (buffer == NULL)) + { + return false; + } + + p.buffer = (unsigned char*)buffer; + p.length = (size_t)length; + p.offset = 0; + p.noalloc = true; + p.format = format; + p.hooks = global_hooks; + + return print_value(item, &p); +} + +/* Parser core - when encountering text, process appropriately. */ +static cJSON_bool parse_value(cJSON * const item, parse_buffer * const input_buffer) +{ + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; /* no input */ + } + + /* parse the different types of values */ + /* null */ + if (can_read(input_buffer, 4) && (strncmp((const char*)buffer_at_offset(input_buffer), "null", 4) == 0)) + { + item->type = cJSON_NULL; + input_buffer->offset += 4; + return true; + } + /* false */ + if (can_read(input_buffer, 5) && (strncmp((const char*)buffer_at_offset(input_buffer), "false", 5) == 0)) + { + item->type = cJSON_False; + input_buffer->offset += 5; + return true; + } + /* true */ + if (can_read(input_buffer, 4) && (strncmp((const char*)buffer_at_offset(input_buffer), "true", 4) == 0)) + { + item->type = cJSON_True; + item->valueint = 1; + input_buffer->offset += 4; + return true; + } + /* string */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '\"')) + { + return parse_string(item, input_buffer); + } + /* number */ + if (can_access_at_index(input_buffer, 0) && ((buffer_at_offset(input_buffer)[0] == '-') || ((buffer_at_offset(input_buffer)[0] >= '0') && (buffer_at_offset(input_buffer)[0] <= '9')))) + { + return parse_number(item, input_buffer); + } + /* array */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '[')) + { + return parse_array(item, input_buffer); + } + /* object */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '{')) + { + return parse_object(item, input_buffer); + } + + return false; +} + +/* Render a value to text. */ +static cJSON_bool print_value(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output = NULL; + + if ((item == NULL) || (output_buffer == NULL)) + { + return false; + } + + switch ((item->type) & 0xFF) + { + case cJSON_NULL: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "null"); + return true; + + case cJSON_False: + output = ensure(output_buffer, 6); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "false"); + return true; + + case cJSON_True: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "true"); + return true; + + case cJSON_Number: + return print_number(item, output_buffer); + + case cJSON_Raw: + { + size_t raw_length = 0; + if (item->valuestring == NULL) + { + return false; + } + + raw_length = strlen(item->valuestring) + sizeof(""); + output = ensure(output_buffer, raw_length); + if (output == NULL) + { + return false; + } + memcpy(output, item->valuestring, raw_length); + return true; + } + + case cJSON_String: + return print_string(item, output_buffer); + + case cJSON_Array: + return print_array(item, output_buffer); + + case cJSON_Object: + return print_object(item, output_buffer); + + default: + return false; + } +} + +/* Build an array from input text. */ +static cJSON_bool parse_array(cJSON * const item, parse_buffer * const input_buffer) +{ + cJSON *head = NULL; /* head of the linked list */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (buffer_at_offset(input_buffer)[0] != '[') + { + /* not an array */ + goto fail; + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ']')) + { + /* empty array */ + goto success; + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + /* parse next value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } + while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || buffer_at_offset(input_buffer)[0] != ']') + { + goto fail; /* expected end of array */ + } + +success: + input_buffer->depth--; + + if (head != NULL) { + head->prev = current_item; + } + + item->type = cJSON_Array; + item->child = head; + + input_buffer->offset++; + + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an array to text */ +static cJSON_bool print_array(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_element = item->child; + + if (output_buffer == NULL) + { + return false; + } + + /* Compose the output array. */ + /* opening square bracket */ + output_pointer = ensure(output_buffer, 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer = '['; + output_buffer->offset++; + output_buffer->depth++; + + while (current_element != NULL) + { + if (!print_value(current_element, output_buffer)) + { + return false; + } + update_offset(output_buffer); + if (current_element->next) + { + length = (size_t) (output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ','; + if(output_buffer->format) + { + *output_pointer++ = ' '; + } + *output_pointer = '\0'; + output_buffer->offset += length; + } + current_element = current_element->next; + } + + output_pointer = ensure(output_buffer, 2); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ']'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Build an object from the text. */ +static cJSON_bool parse_object(cJSON * const item, parse_buffer * const input_buffer) +{ + cJSON *head = NULL; /* linked list head */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '{')) + { + goto fail; /* not an object */ + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '}')) + { + goto success; /* empty object */ + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + /* parse the name of the child */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_string(current_item, input_buffer)) + { + goto fail; /* failed to parse name */ + } + buffer_skip_whitespace(input_buffer); + + /* swap valuestring and string, because we parsed the name */ + current_item->string = current_item->valuestring; + current_item->valuestring = NULL; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != ':')) + { + goto fail; /* invalid object */ + } + + /* parse the value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } + while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '}')) + { + goto fail; /* expected end of object */ + } + +success: + input_buffer->depth--; + + if (head != NULL) { + head->prev = current_item; + } + + item->type = cJSON_Object; + item->child = head; + + input_buffer->offset++; + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an object to text. */ +static cJSON_bool print_object(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_item = item->child; + + if (output_buffer == NULL) + { + return false; + } + + /* Compose the output: */ + length = (size_t) (output_buffer->format ? 2 : 1); /* fmt: {\n */ + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer++ = '{'; + output_buffer->depth++; + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + output_buffer->offset += length; + + while (current_item) + { + if (output_buffer->format) + { + size_t i; + output_pointer = ensure(output_buffer, output_buffer->depth); + if (output_pointer == NULL) + { + return false; + } + for (i = 0; i < output_buffer->depth; i++) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += output_buffer->depth; + } + + /* print key */ + if (!print_string_ptr((unsigned char*)current_item->string, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + length = (size_t) (output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ':'; + if (output_buffer->format) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += length; + + /* print value */ + if (!print_value(current_item, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + /* print comma if not last */ + length = ((size_t)(output_buffer->format ? 1 : 0) + (size_t)(current_item->next ? 1 : 0)); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + if (current_item->next) + { + *output_pointer++ = ','; + } + + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + *output_pointer = '\0'; + output_buffer->offset += length; + + current_item = current_item->next; + } + + output_pointer = ensure(output_buffer, output_buffer->format ? (output_buffer->depth + 1) : 2); + if (output_pointer == NULL) + { + return false; + } + if (output_buffer->format) + { + size_t i; + for (i = 0; i < (output_buffer->depth - 1); i++) + { + *output_pointer++ = '\t'; + } + } + *output_pointer++ = '}'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Get Array size/item / object item. */ +CJSON_PUBLIC(int) cJSON_GetArraySize(const cJSON *array) +{ + cJSON *child = NULL; + size_t size = 0; + + if (array == NULL) + { + return 0; + } + + child = array->child; + + while(child != NULL) + { + size++; + child = child->next; + } + + /* FIXME: Can overflow here. Cannot be fixed without breaking the API */ + + return (int)size; +} + +static cJSON* get_array_item(const cJSON *array, size_t index) +{ + cJSON *current_child = NULL; + + if (array == NULL) + { + return NULL; + } + + current_child = array->child; + while ((current_child != NULL) && (index > 0)) + { + index--; + current_child = current_child->next; + } + + return current_child; +} + +CJSON_PUBLIC(cJSON *) cJSON_GetArrayItem(const cJSON *array, int index) +{ + if (index < 0) + { + return NULL; + } + + return get_array_item(array, (size_t)index); +} + +static cJSON *get_object_item(const cJSON * const object, const char * const name, const cJSON_bool case_sensitive) +{ + cJSON *current_element = NULL; + + if ((object == NULL) || (name == NULL)) + { + return NULL; + } + + current_element = object->child; + if (case_sensitive) + { + while ((current_element != NULL) && (current_element->string != NULL) && (strcmp(name, current_element->string) != 0)) + { + current_element = current_element->next; + } + } + else + { + while ((current_element != NULL) && (case_insensitive_strcmp((const unsigned char*)name, (const unsigned char*)(current_element->string)) != 0)) + { + current_element = current_element->next; + } + } + + if ((current_element == NULL) || (current_element->string == NULL)) { + return NULL; + } + + return current_element; +} + +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItem(const cJSON * const object, const char * const string) +{ + return get_object_item(object, string, false); +} + +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItemCaseSensitive(const cJSON * const object, const char * const string) +{ + return get_object_item(object, string, true); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_HasObjectItem(const cJSON *object, const char *string) +{ + return cJSON_GetObjectItem(object, string) ? 1 : 0; +} + +/* Utility for array list handling. */ +static void suffix_object(cJSON *prev, cJSON *item) +{ + prev->next = item; + item->prev = prev; +} + +/* Utility for handling references. */ +static cJSON *create_reference(const cJSON *item, const internal_hooks * const hooks) +{ + cJSON *reference = NULL; + if (item == NULL) + { + return NULL; + } + + reference = cJSON_New_Item(hooks); + if (reference == NULL) + { + return NULL; + } + + memcpy(reference, item, sizeof(cJSON)); + reference->string = NULL; + reference->type |= cJSON_IsReference; + reference->next = reference->prev = NULL; + return reference; +} + +static cJSON_bool add_item_to_array(cJSON *array, cJSON *item) +{ + cJSON *child = NULL; + + if ((item == NULL) || (array == NULL) || (array == item)) + { + return false; + } + + child = array->child; + /* + * To find the last item in array quickly, we use prev in array + */ + if (child == NULL) + { + /* list is empty, start new one */ + array->child = item; + item->prev = item; + item->next = NULL; + } + else + { + /* append to the end */ + if (child->prev) + { + suffix_object(child->prev, item); + array->child->prev = item; + } + } + + return true; +} + +/* Add item to array/object. */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToArray(cJSON *array, cJSON *item) +{ + return add_item_to_array(array, item); +} + +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) + #pragma GCC diagnostic push +#endif +#ifdef __GNUC__ +#pragma GCC diagnostic ignored "-Wcast-qual" +#endif +/* helper function to cast away const */ +static void* cast_away_const(const void* string) +{ + return (void*)string; +} +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) + #pragma GCC diagnostic pop +#endif + + +static cJSON_bool add_item_to_object(cJSON * const object, const char * const string, cJSON * const item, const internal_hooks * const hooks, const cJSON_bool constant_key) +{ + char *new_key = NULL; + int new_type = cJSON_Invalid; + + if ((object == NULL) || (string == NULL) || (item == NULL) || (object == item)) + { + return false; + } + + if (constant_key) + { + new_key = (char*)cast_away_const(string); + new_type = item->type | cJSON_StringIsConst; + } + else + { + new_key = (char*)cJSON_strdup((const unsigned char*)string, hooks); + if (new_key == NULL) + { + return false; + } + + new_type = item->type & ~cJSON_StringIsConst; + } + + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + hooks->deallocate(item->string); + } + + item->string = new_key; + item->type = new_type; + + return add_item_to_array(object, item); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, false); +} + +/* Add an item to an object with constant string as key */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, true); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item) +{ + if (array == NULL) + { + return false; + } + + return add_item_to_array(array, create_reference(item, &global_hooks)); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item) +{ + if ((object == NULL) || (string == NULL)) + { + return false; + } + + return add_item_to_object(object, string, create_reference(item, &global_hooks), &global_hooks, false); +} + +CJSON_PUBLIC(cJSON*) cJSON_AddNullToObject(cJSON * const object, const char * const name) +{ + cJSON *null = cJSON_CreateNull(); + if (add_item_to_object(object, name, null, &global_hooks, false)) + { + return null; + } + + cJSON_Delete(null); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddTrueToObject(cJSON * const object, const char * const name) +{ + cJSON *true_item = cJSON_CreateTrue(); + if (add_item_to_object(object, name, true_item, &global_hooks, false)) + { + return true_item; + } + + cJSON_Delete(true_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddFalseToObject(cJSON * const object, const char * const name) +{ + cJSON *false_item = cJSON_CreateFalse(); + if (add_item_to_object(object, name, false_item, &global_hooks, false)) + { + return false_item; + } + + cJSON_Delete(false_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddBoolToObject(cJSON * const object, const char * const name, const cJSON_bool boolean) +{ + cJSON *bool_item = cJSON_CreateBool(boolean); + if (add_item_to_object(object, name, bool_item, &global_hooks, false)) + { + return bool_item; + } + + cJSON_Delete(bool_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddNumberToObject(cJSON * const object, const char * const name, const double number) +{ + cJSON *number_item = cJSON_CreateNumber(number); + if (add_item_to_object(object, name, number_item, &global_hooks, false)) + { + return number_item; + } + + cJSON_Delete(number_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddStringToObject(cJSON * const object, const char * const name, const char * const string) +{ + cJSON *string_item = cJSON_CreateString(string); + if (add_item_to_object(object, name, string_item, &global_hooks, false)) + { + return string_item; + } + + cJSON_Delete(string_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddRawToObject(cJSON * const object, const char * const name, const char * const raw) +{ + cJSON *raw_item = cJSON_CreateRaw(raw); + if (add_item_to_object(object, name, raw_item, &global_hooks, false)) + { + return raw_item; + } + + cJSON_Delete(raw_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddObjectToObject(cJSON * const object, const char * const name) +{ + cJSON *object_item = cJSON_CreateObject(); + if (add_item_to_object(object, name, object_item, &global_hooks, false)) + { + return object_item; + } + + cJSON_Delete(object_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddArrayToObject(cJSON * const object, const char * const name) +{ + cJSON *array = cJSON_CreateArray(); + if (add_item_to_object(object, name, array, &global_hooks, false)) + { + return array; + } + + cJSON_Delete(array); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemViaPointer(cJSON *parent, cJSON * const item) +{ + if ((parent == NULL) || (item == NULL)) + { + return NULL; + } + + if (item != parent->child) + { + /* not the first element */ + item->prev->next = item->next; + } + if (item->next != NULL) + { + /* not the last element */ + item->next->prev = item->prev; + } + + if (item == parent->child) + { + /* first element */ + parent->child = item->next; + } + else if (item->next == NULL) + { + /* last element */ + parent->child->prev = item->prev; + } + + /* make sure the detached item doesn't point anywhere anymore */ + item->prev = NULL; + item->next = NULL; + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromArray(cJSON *array, int which) +{ + if (which < 0) + { + return NULL; + } + + return cJSON_DetachItemViaPointer(array, get_array_item(array, (size_t)which)); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromArray(cJSON *array, int which) +{ + cJSON_Delete(cJSON_DetachItemFromArray(array, which)); +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObject(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItem(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItemCaseSensitive(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromObject(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObject(object, string)); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObjectCaseSensitive(object, string)); +} + +/* Replace array/object items with new ones. */ +CJSON_PUBLIC(cJSON_bool) cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem) +{ + cJSON *after_inserted = NULL; + + if (which < 0) + { + return false; + } + + after_inserted = get_array_item(array, (size_t)which); + if (after_inserted == NULL) + { + return add_item_to_array(array, newitem); + } + + newitem->next = after_inserted; + newitem->prev = after_inserted->prev; + after_inserted->prev = newitem; + if (after_inserted == array->child) + { + array->child = newitem; + } + else + { + newitem->prev->next = newitem; + } + return true; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemViaPointer(cJSON * const parent, cJSON * const item, cJSON * replacement) +{ + if ((parent == NULL) || (replacement == NULL) || (item == NULL)) + { + return false; + } + + if (replacement == item) + { + return true; + } + + replacement->next = item->next; + replacement->prev = item->prev; + + if (replacement->next != NULL) + { + replacement->next->prev = replacement; + } + if (parent->child == item) + { + if (parent->child->prev == parent->child) + { + replacement->prev = replacement; + } + parent->child = replacement; + } + else + { /* + * To find the last item in array quickly, we use prev in array. + * We can't modify the last item's next pointer where this item was the parent's child + */ + if (replacement->prev != NULL) + { + replacement->prev->next = replacement; + } + if (replacement->next == NULL) + { + parent->child->prev = replacement; + } + } + + item->next = NULL; + item->prev = NULL; + cJSON_Delete(item); + + return true; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem) +{ + if (which < 0) + { + return false; + } + + return cJSON_ReplaceItemViaPointer(array, get_array_item(array, (size_t)which), newitem); +} + +static cJSON_bool replace_item_in_object(cJSON *object, const char *string, cJSON *replacement, cJSON_bool case_sensitive) +{ + if ((replacement == NULL) || (string == NULL)) + { + return false; + } + + /* replace the name in the replacement */ + if (!(replacement->type & cJSON_StringIsConst) && (replacement->string != NULL)) + { + cJSON_free(replacement->string); + } + replacement->string = (char*)cJSON_strdup((const unsigned char*)string, &global_hooks); + if (replacement->string == NULL) + { + return false; + } + + replacement->type &= ~cJSON_StringIsConst; + + return cJSON_ReplaceItemViaPointer(object, get_object_item(object, string, case_sensitive), replacement); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObject(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, false); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, true); +} + +/* Create basic types: */ +CJSON_PUBLIC(cJSON *) cJSON_CreateNull(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_NULL; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateTrue(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_True; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateFalse(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateBool(cJSON_bool boolean) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = boolean ? cJSON_True : cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateNumber(double num) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_Number; + item->valuedouble = num; + + /* use saturation in case of overflow */ + if (num >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (num <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)num; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateString(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_String; + item->valuestring = (char*)cJSON_strdup((const unsigned char*)string, &global_hooks); + if(!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateStringReference(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_String | cJSON_IsReference; + item->valuestring = (char*)cast_away_const(string); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateObjectReference(const cJSON *child) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) { + item->type = cJSON_Object | cJSON_IsReference; + item->child = (cJSON*)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateArrayReference(const cJSON *child) { + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) { + item->type = cJSON_Array | cJSON_IsReference; + item->child = (cJSON*)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateRaw(const char *raw) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_Raw; + item->valuestring = (char*)cJSON_strdup((const unsigned char*)raw, &global_hooks); + if(!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateArray(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type=cJSON_Array; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateObject(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Object; + } + + return item; +} + +/* Create Arrays: */ +CJSON_PUBLIC(cJSON *) cJSON_CreateIntArray(const int *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for(i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateFloatArray(const float *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for(i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber((double)numbers[i]); + if(!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateDoubleArray(const double *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for(i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if(!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateStringArray(const char *const *strings, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (strings == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateString(strings[i]); + if(!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p,n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +/* Duplication */ +CJSON_PUBLIC(cJSON *) cJSON_Duplicate(const cJSON *item, cJSON_bool recurse) +{ + cJSON *newitem = NULL; + cJSON *child = NULL; + cJSON *next = NULL; + cJSON *newchild = NULL; + + /* Bail on bad ptr */ + if (!item) + { + goto fail; + } + /* Create new item */ + newitem = cJSON_New_Item(&global_hooks); + if (!newitem) + { + goto fail; + } + /* Copy over all vars */ + newitem->type = item->type & (~cJSON_IsReference); + newitem->valueint = item->valueint; + newitem->valuedouble = item->valuedouble; + if (item->valuestring) + { + newitem->valuestring = (char*)cJSON_strdup((unsigned char*)item->valuestring, &global_hooks); + if (!newitem->valuestring) + { + goto fail; + } + } + if (item->string) + { + newitem->string = (item->type&cJSON_StringIsConst) ? item->string : (char*)cJSON_strdup((unsigned char*)item->string, &global_hooks); + if (!newitem->string) + { + goto fail; + } + } + /* If non-recursive, then we're done! */ + if (!recurse) + { + return newitem; + } + /* Walk the ->next chain for the child. */ + child = item->child; + while (child != NULL) + { + newchild = cJSON_Duplicate(child, true); /* Duplicate (with recurse) each item in the ->next chain */ + if (!newchild) + { + goto fail; + } + if (next != NULL) + { + /* If newitem->child already set, then crosswire ->prev and ->next and move on */ + next->next = newchild; + newchild->prev = next; + next = newchild; + } + else + { + /* Set newitem->child and move to it */ + newitem->child = newchild; + next = newchild; + } + child = child->next; + } + if (newitem && newitem->child) + { + newitem->child->prev = newchild; + } + + return newitem; + +fail: + if (newitem != NULL) + { + cJSON_Delete(newitem); + } + + return NULL; +} + +static void skip_oneline_comment(char **input) +{ + *input += static_strlen("//"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if ((*input)[0] == '\n') { + *input += static_strlen("\n"); + return; + } + } +} + +static void skip_multiline_comment(char **input) +{ + *input += static_strlen("/*"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if (((*input)[0] == '*') && ((*input)[1] == '/')) + { + *input += static_strlen("*/"); + return; + } + } +} + +static void minify_string(char **input, char **output) { + (*output)[0] = (*input)[0]; + *input += static_strlen("\""); + *output += static_strlen("\""); + + + for (; (*input)[0] != '\0'; (void)++(*input), ++(*output)) { + (*output)[0] = (*input)[0]; + + if ((*input)[0] == '\"') { + (*output)[0] = '\"'; + *input += static_strlen("\""); + *output += static_strlen("\""); + return; + } else if (((*input)[0] == '\\') && ((*input)[1] == '\"')) { + (*output)[1] = (*input)[1]; + *input += static_strlen("\""); + *output += static_strlen("\""); + } + } +} + +CJSON_PUBLIC(void) cJSON_Minify(char *json) +{ + char *into = json; + + if (json == NULL) + { + return; + } + + while (json[0] != '\0') + { + switch (json[0]) + { + case ' ': + case '\t': + case '\r': + case '\n': + json++; + break; + + case '/': + if (json[1] == '/') + { + skip_oneline_comment(&json); + } + else if (json[1] == '*') + { + skip_multiline_comment(&json); + } else { + json++; + } + break; + + case '\"': + minify_string(&json, (char**)&into); + break; + + default: + into[0] = json[0]; + json++; + into++; + } + } + + /* and null-terminate. */ + *into = '\0'; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsInvalid(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Invalid; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsFalse(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_False; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsTrue(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xff) == cJSON_True; +} + + +CJSON_PUBLIC(cJSON_bool) cJSON_IsBool(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & (cJSON_True | cJSON_False)) != 0; +} +CJSON_PUBLIC(cJSON_bool) cJSON_IsNull(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_NULL; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsNumber(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Number; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsString(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_String; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsArray(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Array; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsObject(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Object; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsRaw(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Raw; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_Compare(const cJSON * const a, const cJSON * const b, const cJSON_bool case_sensitive) +{ + if ((a == NULL) || (b == NULL) || ((a->type & 0xFF) != (b->type & 0xFF))) + { + return false; + } + + /* check if type is valid */ + switch (a->type & 0xFF) + { + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + case cJSON_Number: + case cJSON_String: + case cJSON_Raw: + case cJSON_Array: + case cJSON_Object: + break; + + default: + return false; + } + + /* identical objects are equal */ + if (a == b) + { + return true; + } + + switch (a->type & 0xFF) + { + /* in these cases and equal type is enough */ + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + return true; + + case cJSON_Number: + if (compare_double(a->valuedouble, b->valuedouble)) + { + return true; + } + return false; + + case cJSON_String: + case cJSON_Raw: + if ((a->valuestring == NULL) || (b->valuestring == NULL)) + { + return false; + } + if (strcmp(a->valuestring, b->valuestring) == 0) + { + return true; + } + + return false; + + case cJSON_Array: + { + cJSON *a_element = a->child; + cJSON *b_element = b->child; + + for (; (a_element != NULL) && (b_element != NULL);) + { + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + + a_element = a_element->next; + b_element = b_element->next; + } + + /* one of the arrays is longer than the other */ + if (a_element != b_element) { + return false; + } + + return true; + } + + case cJSON_Object: + { + cJSON *a_element = NULL; + cJSON *b_element = NULL; + cJSON_ArrayForEach(a_element, a) + { + /* TODO This has O(n^2) runtime, which is horrible! */ + b_element = get_object_item(b, a_element->string, case_sensitive); + if (b_element == NULL) + { + return false; + } + + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + } + + /* doing this twice, once on a and b to prevent true comparison if a subset of b + * TODO: Do this the proper way, this is just a fix for now */ + cJSON_ArrayForEach(b_element, b) + { + a_element = get_object_item(a, b_element->string, case_sensitive); + if (a_element == NULL) + { + return false; + } + + if (!cJSON_Compare(b_element, a_element, case_sensitive)) + { + return false; + } + } + + return true; + } + + default: + return false; + } +} + +CJSON_PUBLIC(void *) cJSON_malloc(size_t size) +{ + return global_hooks.allocate(size); +} + +CJSON_PUBLIC(void) cJSON_free(void *object) +{ + global_hooks.deallocate(object); +} diff --git a/core/json/cJSON.h b/core/json/cJSON.h new file mode 100644 index 0000000..0669975 --- /dev/null +++ b/core/json/cJSON.h @@ -0,0 +1,300 @@ +/* + Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef cJSON__h +#define cJSON__h + +#ifdef __cplusplus +extern "C" +{ +#endif + +#if !defined(__WINDOWS__) && (defined(WIN32) || defined(WIN64) || defined(_MSC_VER) || defined(_WIN32)) +#define __WINDOWS__ +#endif + +#ifdef __WINDOWS__ + +/* When compiling for windows, we specify a specific calling convention to avoid issues where we are being called from a project with a different default calling convention. For windows you have 3 define options: + +CJSON_HIDE_SYMBOLS - Define this in the case where you don't want to ever dllexport symbols +CJSON_EXPORT_SYMBOLS - Define this on library build when you want to dllexport symbols (default) +CJSON_IMPORT_SYMBOLS - Define this if you want to dllimport symbol + +For *nix builds that support visibility attribute, you can define similar behavior by + +setting default visibility to hidden by adding +-fvisibility=hidden (for gcc) +or +-xldscope=hidden (for sun cc) +to CFLAGS + +then using the CJSON_API_VISIBILITY flag to "export" the same symbols the way CJSON_EXPORT_SYMBOLS does + +*/ + +#define CJSON_CDECL __cdecl +#define CJSON_STDCALL __stdcall + +/* export symbols by default, this is necessary for copy pasting the C and header file */ +#if !defined(CJSON_HIDE_SYMBOLS) && !defined(CJSON_IMPORT_SYMBOLS) && !defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_EXPORT_SYMBOLS +#endif + +#if defined(CJSON_HIDE_SYMBOLS) +#define CJSON_PUBLIC(type) type CJSON_STDCALL +#elif defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllexport) type CJSON_STDCALL +#elif defined(CJSON_IMPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllimport) type CJSON_STDCALL +#endif +#else /* !__WINDOWS__ */ +#define CJSON_CDECL +#define CJSON_STDCALL + +#if (defined(__GNUC__) || defined(__SUNPRO_CC) || defined (__SUNPRO_C)) && defined(CJSON_API_VISIBILITY) +#define CJSON_PUBLIC(type) __attribute__((visibility("default"))) type +#else +#define CJSON_PUBLIC(type) type +#endif +#endif + +/* project version */ +#define CJSON_VERSION_MAJOR 1 +#define CJSON_VERSION_MINOR 7 +#define CJSON_VERSION_PATCH 15 + +#include + +/* cJSON Types: */ +#define cJSON_Invalid (0) +#define cJSON_False (1 << 0) +#define cJSON_True (1 << 1) +#define cJSON_NULL (1 << 2) +#define cJSON_Number (1 << 3) +#define cJSON_String (1 << 4) +#define cJSON_Array (1 << 5) +#define cJSON_Object (1 << 6) +#define cJSON_Raw (1 << 7) /* raw json */ + +#define cJSON_IsReference 256 +#define cJSON_StringIsConst 512 + +/* The cJSON structure: */ +typedef struct cJSON +{ + /* next/prev allow you to walk array/object chains. Alternatively, use GetArraySize/GetArrayItem/GetObjectItem */ + struct cJSON *next; + struct cJSON *prev; + /* An array or object item will have a child pointer pointing to a chain of the items in the array/object. */ + struct cJSON *child; + + /* The type of the item, as above. */ + int type; + + /* The item's string, if type==cJSON_String and type == cJSON_Raw */ + char *valuestring; + /* writing to valueint is DEPRECATED, use cJSON_SetNumberValue instead */ + int valueint; + /* The item's number, if type==cJSON_Number */ + double valuedouble; + + /* The item's name string, if this item is the child of, or is in the list of subitems of an object. */ + char *string; +} cJSON; + +typedef struct cJSON_Hooks +{ + /* malloc/free are CDECL on Windows regardless of the default calling convention of the compiler, so ensure the hooks allow passing those functions directly. */ + void *(CJSON_CDECL *malloc_fn)(size_t sz); + void (CJSON_CDECL *free_fn)(void *ptr); +} cJSON_Hooks; + +typedef int cJSON_bool; + +/* Limits how deeply nested arrays/objects can be before cJSON rejects to parse them. + * This is to prevent stack overflows. */ +#ifndef CJSON_NESTING_LIMIT +#define CJSON_NESTING_LIMIT 1000 +#endif + +/* returns the version of cJSON as a string */ +CJSON_PUBLIC(const char*) cJSON_Version(void); + +/* Supply malloc, realloc and free functions to cJSON */ +CJSON_PUBLIC(void) cJSON_InitHooks(cJSON_Hooks* hooks); + +/* Memory Management: the caller is always responsible to free the results from all variants of cJSON_Parse (with cJSON_Delete) and cJSON_Print (with stdlib free, cJSON_Hooks.free_fn, or cJSON_free as appropriate). The exception is cJSON_PrintPreallocated, where the caller has full responsibility of the buffer. */ +/* Supply a block of JSON, and this returns a cJSON object you can interrogate. */ +CJSON_PUBLIC(cJSON *) cJSON_Parse(const char *value); +CJSON_PUBLIC(cJSON *) cJSON_ParseWithLength(const char *value, size_t buffer_length); +/* ParseWithOpts allows you to require (and check) that the JSON is null terminated, and to retrieve the pointer to the final byte parsed. */ +/* If you supply a ptr in return_parse_end and parsing fails, then return_parse_end will contain a pointer to the error so will match cJSON_GetErrorPtr(). */ +CJSON_PUBLIC(cJSON *) cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated); +CJSON_PUBLIC(cJSON *) cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated); + +/* Render a cJSON entity to text for transfer/storage. */ +CJSON_PUBLIC(char *) cJSON_Print(const cJSON *item); +/* Render a cJSON entity to text for transfer/storage without any formatting. */ +CJSON_PUBLIC(char *) cJSON_PrintUnformatted(const cJSON *item); +/* Render a cJSON entity to text using a buffered strategy. prebuffer is a guess at the final size. guessing well reduces reallocation. fmt=0 gives unformatted, =1 gives formatted */ +CJSON_PUBLIC(char *) cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt); +/* Render a cJSON entity to text using a buffer already allocated in memory with given length. Returns 1 on success and 0 on failure. */ +/* NOTE: cJSON is not always 100% accurate in estimating how much memory it will use, so to be safe allocate 5 bytes more than you actually need */ +CJSON_PUBLIC(cJSON_bool) cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format); +/* Delete a cJSON entity and all subentities. */ +CJSON_PUBLIC(void) cJSON_Delete(cJSON *item); + +/* Returns the number of items in an array (or object). */ +CJSON_PUBLIC(int) cJSON_GetArraySize(const cJSON *array); +/* Retrieve item number "index" from array "array". Returns NULL if unsuccessful. */ +CJSON_PUBLIC(cJSON *) cJSON_GetArrayItem(const cJSON *array, int index); +/* Get item "string" from object. Case insensitive. */ +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItem(const cJSON * const object, const char * const string); +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItemCaseSensitive(const cJSON * const object, const char * const string); +CJSON_PUBLIC(cJSON_bool) cJSON_HasObjectItem(const cJSON *object, const char *string); +/* For analysing failed parses. This returns a pointer to the parse error. You'll probably need to look a few chars back to make sense of it. Defined when cJSON_Parse() returns 0. 0 when cJSON_Parse() succeeds. */ +CJSON_PUBLIC(const char *) cJSON_GetErrorPtr(void); + +/* Check item type and return its value */ +CJSON_PUBLIC(char *) cJSON_GetStringValue(const cJSON * const item); +CJSON_PUBLIC(double) cJSON_GetNumberValue(const cJSON * const item); + +/* These functions check the type of an item */ +CJSON_PUBLIC(cJSON_bool) cJSON_IsInvalid(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsFalse(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsTrue(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsBool(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsNull(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsNumber(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsString(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsArray(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsObject(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsRaw(const cJSON * const item); + +/* These calls create a cJSON item of the appropriate type. */ +CJSON_PUBLIC(cJSON *) cJSON_CreateNull(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateTrue(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateFalse(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateBool(cJSON_bool boolean); +CJSON_PUBLIC(cJSON *) cJSON_CreateNumber(double num); +CJSON_PUBLIC(cJSON *) cJSON_CreateString(const char *string); +/* raw json */ +CJSON_PUBLIC(cJSON *) cJSON_CreateRaw(const char *raw); +CJSON_PUBLIC(cJSON *) cJSON_CreateArray(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateObject(void); + +/* Create a string where valuestring references a string so + * it will not be freed by cJSON_Delete */ +CJSON_PUBLIC(cJSON *) cJSON_CreateStringReference(const char *string); +/* Create an object/array that only references it's elements so + * they will not be freed by cJSON_Delete */ +CJSON_PUBLIC(cJSON *) cJSON_CreateObjectReference(const cJSON *child); +CJSON_PUBLIC(cJSON *) cJSON_CreateArrayReference(const cJSON *child); + +/* These utilities create an Array of count items. + * The parameter count cannot be greater than the number of elements in the number array, otherwise array access will be out of bounds.*/ +CJSON_PUBLIC(cJSON *) cJSON_CreateIntArray(const int *numbers, int count); +CJSON_PUBLIC(cJSON *) cJSON_CreateFloatArray(const float *numbers, int count); +CJSON_PUBLIC(cJSON *) cJSON_CreateDoubleArray(const double *numbers, int count); +CJSON_PUBLIC(cJSON *) cJSON_CreateStringArray(const char *const *strings, int count); + +/* Append item to the specified array/object. */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToArray(cJSON *array, cJSON *item); +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item); +/* Use this when string is definitely const (i.e. a literal, or as good as), and will definitely survive the cJSON object. + * WARNING: When this function was used, make sure to always check that (item->type & cJSON_StringIsConst) is zero before + * writing to `item->string` */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item); +/* Append reference to item to the specified array/object. Use this when you want to add an existing cJSON to a new cJSON, but don't want to corrupt your existing cJSON. */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item); +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item); + +/* Remove/Detach items from Arrays/Objects. */ +CJSON_PUBLIC(cJSON *) cJSON_DetachItemViaPointer(cJSON *parent, cJSON * const item); +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromArray(cJSON *array, int which); +CJSON_PUBLIC(void) cJSON_DeleteItemFromArray(cJSON *array, int which); +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObject(cJSON *object, const char *string); +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string); +CJSON_PUBLIC(void) cJSON_DeleteItemFromObject(cJSON *object, const char *string); +CJSON_PUBLIC(void) cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string); + +/* Update array items. */ +CJSON_PUBLIC(cJSON_bool) cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem); /* Shifts pre-existing items to the right. */ +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemViaPointer(cJSON * const parent, cJSON * const item, cJSON * replacement); +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem); +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObject(cJSON *object,const char *string,cJSON *newitem); +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object,const char *string,cJSON *newitem); + +/* Duplicate a cJSON item */ +CJSON_PUBLIC(cJSON *) cJSON_Duplicate(const cJSON *item, cJSON_bool recurse); +/* Duplicate will create a new, identical cJSON item to the one you pass, in new memory that will + * need to be released. With recurse!=0, it will duplicate any children connected to the item. + * The item->next and ->prev pointers are always zero on return from Duplicate. */ +/* Recursively compare two cJSON items for equality. If either a or b is NULL or invalid, they will be considered unequal. + * case_sensitive determines if object keys are treated case sensitive (1) or case insensitive (0) */ +CJSON_PUBLIC(cJSON_bool) cJSON_Compare(const cJSON * const a, const cJSON * const b, const cJSON_bool case_sensitive); + +/* Minify a strings, remove blank characters(such as ' ', '\t', '\r', '\n') from strings. + * The input pointer json cannot point to a read-only address area, such as a string constant, + * but should point to a readable and writable address area. */ +CJSON_PUBLIC(void) cJSON_Minify(char *json); + +/* Helper functions for creating and adding items to an object at the same time. + * They return the added item or NULL on failure. */ +CJSON_PUBLIC(cJSON*) cJSON_AddNullToObject(cJSON * const object, const char * const name); +CJSON_PUBLIC(cJSON*) cJSON_AddTrueToObject(cJSON * const object, const char * const name); +CJSON_PUBLIC(cJSON*) cJSON_AddFalseToObject(cJSON * const object, const char * const name); +CJSON_PUBLIC(cJSON*) cJSON_AddBoolToObject(cJSON * const object, const char * const name, const cJSON_bool boolean); +CJSON_PUBLIC(cJSON*) cJSON_AddNumberToObject(cJSON * const object, const char * const name, const double number); +CJSON_PUBLIC(cJSON*) cJSON_AddStringToObject(cJSON * const object, const char * const name, const char * const string); +CJSON_PUBLIC(cJSON*) cJSON_AddRawToObject(cJSON * const object, const char * const name, const char * const raw); +CJSON_PUBLIC(cJSON*) cJSON_AddObjectToObject(cJSON * const object, const char * const name); +CJSON_PUBLIC(cJSON*) cJSON_AddArrayToObject(cJSON * const object, const char * const name); + +/* When assigning an integer value, it needs to be propagated to valuedouble too. */ +#define cJSON_SetIntValue(object, number) ((object) ? (object)->valueint = (object)->valuedouble = (number) : (number)) +/* helper for the cJSON_SetNumberValue macro */ +CJSON_PUBLIC(double) cJSON_SetNumberHelper(cJSON *object, double number); +#define cJSON_SetNumberValue(object, number) ((object != NULL) ? cJSON_SetNumberHelper(object, (double)number) : (number)) +/* Change the valuestring of a cJSON_String object, only takes effect when type of object is cJSON_String */ +CJSON_PUBLIC(char*) cJSON_SetValuestring(cJSON *object, const char *valuestring); + +/* If the object is not a boolean type this does nothing and returns cJSON_Invalid else it returns the new type*/ +#define cJSON_SetBoolValue(object, boolValue) ( \ + (object != NULL && ((object)->type & (cJSON_False|cJSON_True))) ? \ + (object)->type=((object)->type &(~(cJSON_False|cJSON_True)))|((boolValue)?cJSON_True:cJSON_False) : \ + cJSON_Invalid\ +) + +/* Macro for iterating over an array or object */ +#define cJSON_ArrayForEach(element, array) for(element = (array != NULL) ? (array)->child : NULL; element != NULL; element = element->next) + +/* malloc/free objects using the malloc/free functions that have been set with cJSON_InitHooks */ +CJSON_PUBLIC(void *) cJSON_malloc(size_t size); +CJSON_PUBLIC(void) cJSON_free(void *object); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/examples/gs_test2.cpp b/examples/gs_test2.cpp index 00198ae..9e8a7f0 100644 --- a/examples/gs_test2.cpp +++ b/examples/gs_test2.cpp @@ -40,7 +40,7 @@ struct YdGsOutParamItem struct YdGsOutParam { YdGsRigParam rp; - YdGsOutParamItem items[GS_MAXPOINTSIZE]; + YdGsOutParamItem items[GS_PACKMAXNODES]; YdGsOutParam() { memset(items, 0, sizeof(items)); @@ -358,7 +358,7 @@ bool parseCsv(const std::string& name, YdGsOutParam& op) // << op.items[index].k0 << " " // << op.items[index].k1 << std::endl; index ++; - if (index >= GS_MAXPOINTSIZE) + if (index >= GS_PACKMAXNODES) break; } else //解析失败 @@ -375,7 +375,7 @@ bool parseCsv(const std::string& name, YdGsOutParam& op) bool to3D(const LaserScan& scan, const YdGsOutParam& op, Yd3DPoints& out) { int size = scan.points.size(); - if (size > GS_MAXPOINTSIZE) + if (size > GS_PACKMAXNODES) { std::cout << "点云数过大 " << size << std::endl; return false; diff --git a/examples/sdm_test.cpp b/examples/sdm_test.cpp index 2c903e9..0d39b4a 100644 --- a/examples/sdm_test.cpp +++ b/examples/sdm_test.cpp @@ -36,40 +36,22 @@ #include #include #include +#include "core/common/ydlidar_help.h" + using namespace std; using namespace ydlidar; +using namespace ydlidar::core::common; #if defined(_MSC_VER) #pragma comment(lib, "ydlidar_sdk.lib") #endif -/** - * @brief sdm test - * @param argc - * @param argv - * @return - * @par Flow chart - * Step1: instance CYdLidar.\n - * Step2: set paramters.\n - * Step3: initialize SDK and LiDAR.(::CYdLidar::initialize)\n - * Step4: Start the device scanning routine which runs on a separate thread and enable motor.(::CYdLidar::turnOn)\n - * Step5: Get the LiDAR Scan Data.(::CYdLidar::doProcessSimple)\n - * Step6: Stop the device scanning thread and disable motor.(::CYdLidar::turnOff)\n - * Step7: Uninitialize the SDK and Disconnect the LiDAR.(::CYdLidar::disconnecting)\n - */ int main(int argc, char *argv[]) { - printf("__ ______ _ ___ ____ _ ____ \n"); - printf("\\ \\ / / _ \\| | |_ _| _ \\ / \\ | _ \\ \n"); - printf(" \\ V /| | | | | | || | | |/ _ \\ | |_) | \n"); - printf(" | | | |_| | |___ | || |_| / ___ \\| _ < \n"); - printf(" |_| |____/|_____|___|____/_/ \\_\\_| \\_\\ \n"); - printf("\n"); - fflush(stdout); - + printLogo(); //初始化 - ydlidar::os_init(); + os_init(); //初始化串口号 std::string port; std::map ports = ydlidar::lidarPortList(); @@ -95,7 +77,7 @@ int main(int argc, char *argv[]) } else { - while (ydlidar::os_isOk()) + while (os_isOk()) { printf("Please select the lidar port:"); std::string number; diff --git a/examples/tia_test.cpp b/examples/tia_test.cpp new file mode 100644 index 0000000..8707ac5 --- /dev/null +++ b/examples/tia_test.cpp @@ -0,0 +1,129 @@ +#include +#include +#include +#include +#include "CYdLidar.h" +#include "core/common/ydlidar_help.h" + +using namespace std; +using namespace ydlidar; +using namespace ydlidar::core::common; + +#if defined(_MSC_VER) +#pragma comment(lib, "ydlidar_sdk.lib") +#endif + + +int main(int argc, char *argv[]) +{ + printLogo(); + os_init(); + + std::string port = "192.168.0.11"; + int baudrate = 8090; + bool isSingleChannel = false; + float frequency = 30.0f; //TIA(10~30Hz),TIA-H(100Hz,150Hz) + float samplerate = 20.0f; //TIA(10~30Hz),TIA-H(100Hz,300Hz) + + /// instance + CYdLidar laser; + //////////////////////string property///////////////// + /// lidar port + laser.setlidaropt(LidarPropSerialPort, port.c_str(), port.size()); + /// ignore array + std::string ignore_array; + ignore_array.clear(); + laser.setlidaropt(LidarPropIgnoreArray, ignore_array.c_str(), + ignore_array.size()); + + //////////////////////int property///////////////// + /// lidar baudrate + laser.setlidaropt(LidarPropSerialBaudrate, &baudrate, sizeof(int)); + /// tof lidar + int optval = TYPE_TIA; + laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int)); + /// device type + optval = YDLIDAR_TYPE_TCP; + laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int)); + /// sample rate + optval = samplerate; + laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int)); + /// abnormal count + optval = 4; + laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int)); +// optval = 16; +// laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int)); + + //////////////////////bool property///////////////// + /// fixed angle resolution + bool b_optvalue = false; + laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool)); + /// rotate 180 + b_optvalue = false; + laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool)); + /// Counterclockwise + b_optvalue = false; + laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool)); + b_optvalue = true; + laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool)); + /// one-way communication + b_optvalue = isSingleChannel; + laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool)); + /// intensity + b_optvalue = true; + laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool)); + /// Motor DTR + b_optvalue = false; + laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool)); + + //////////////////////float property///////////////// + /// unit: ° + float f_optvalue = 180.0f; + laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float)); + f_optvalue = -180.0f; + laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float)); + /// unit: m + f_optvalue = 64.f; + laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float)); + f_optvalue = 0.05f; + laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float)); + /// unit: Hz + laser.setlidaropt(LidarPropScanFrequency, &frequency, sizeof(float)); + + //启用调试 + // laser.setEnableDebug(true); + + /// initialize SDK and LiDAR. + bool ret = laser.initialize(); + if (ret) + { + /// Start the device scanning routine which runs on a separate thread and enable motor. + ret = laser.turnOn(); + } + if (!ret) + { + error("Error %s", laser.DescribeError()); + return -1; + } + + LaserScan scan; //点云 + while (ret && ydlidar::os_isOk()) + { + if (laser.doProcessSimple(scan)) + { + info("Scan received at [%.02f]Hz [%u] points in [%.03f]s", + scan.scanFreq, + uint(scan.points.size()), + scan.config.scan_time); + } + else + { + error("Failed to get lidar data"); + } + } + + laser.turnOff(); + laser.disconnecting(); + + return 0; +} diff --git a/src/CYdLidar.cpp b/src/CYdLidar.cpp index 7c2d7b8..06522ab 100644 --- a/src/CYdLidar.cpp +++ b/src/CYdLidar.cpp @@ -37,6 +37,7 @@ #include "GSLidarDriver.h" #include "SDMLidarDriver.h" #include "DTSLidarDriver.h" +#include "TiaLidarDriver.h" using namespace std; using namespace impl; @@ -62,10 +63,7 @@ CYdLidar::CYdLidar() : lidarPtr(nullptr) m_MaxRange = 64.0; m_MinRange = 0.01f; m_SampleRate = 5; - // m_SampleRatebyD1 = 5; - // defalutSampleRate = 5; m_ScanFrequency = 10; - scanning = false; m_FixedSize = 720; frequencyOffset = 0.4f; m_AbnormalCheckCount = 2; @@ -418,25 +416,20 @@ bool CYdLidar::getlidaropt(int optname, void *optval, int optlen) bool CYdLidar::initialize() { uint32_t t = getms(); - if (!checkCOMMs()) + if (!checkConnect()) { - fprintf(stderr, - "[YDLIDAR] Error initializing YDLIDAR check Comms.\n"); - fflush(stderr); + error("Error initializing YDLIDAR check Comms."); return false; } if (!checkStatus()) { - fprintf(stderr, - "[YDLIDAR] Error initializing YDLIDAR check status under [%s] and [%d].\n", + error("Error initializing YDLIDAR check status under [%s] and [%d].", m_SerialPort.c_str(), m_SerialBaudrate); - fflush(stderr); return false; } - printf("[YDLIDAR] Lidar init success, Elapsed time %u ms\n", getms() - t); - fflush(stdout); + info("Lidar init success, Elapsed time [%u]ms", getms() - t); return true; } @@ -447,18 +440,18 @@ void CYdLidar::GetLidarVersion(LidarVersion &lv) { memcpy(&lv, &m_LidarVersion, sizeof(LidarVersion)); - printf("[YDLIDAR] lidar version\n" + std::string sn; + for (int i = 0; i < SDK_SNLEN; i++) + sn += char(lv.sn[i] + 48); + info("Lidar version\n" "Firmware version: %u.%u.%u\n" "Hardware version: %u\n" - "Serial: ", + "Serial: %s", lv.soft_major, lv.soft_minor, lv.soft_patch, - lv.hardware); - for (int i = 0; i < SDK_SNLEN; i++) - printf("%01X", lv.sn[i] & 0xff); - printf("\n"); - fflush(stdout); + lv.hardware, + sn.c_str()); } /*------------------------------------------------------------- @@ -466,67 +459,45 @@ void CYdLidar::GetLidarVersion(LidarVersion &lv) -------------------------------------------------------------*/ bool CYdLidar::turnOn() { - //重置错误 - lidarPtr->setDriverError(NoError); - - if (scanning && lidarPtr->isscanning()) - { + if (lidarPtr->isscanning()) return true; - } uint32_t t = getms(); //启动扫描 - result_t op_result = lidarPtr->startScan(); - if (!IS_OK(op_result)) + result_t ret = lidarPtr->startScan(); + if (!IS_OK(ret)) { - op_result = lidarPtr->startScan(); - if (!IS_OK(op_result)) + ret = lidarPtr->startScan(); + if (!IS_OK(ret)) { lidarPtr->stop(); - fprintf(stderr, "[YDLIDAR] Failed to start scan mode: %x\n", op_result); - scanning = false; + error("Failed to start scan mode %d", ret); return false; } } - printf("[YDLIDAR] Successed to start scan mode, Elapsed time %u ms\n", getms() - t); - fflush(stdout); + info("Successed to start scan mode, Elapsed time %u ms", getms() - t); t = getms(); //计算采样率 if (!checkLidarAbnormal()) { lidarPtr->stop(); - fprintf(stderr, - "[YDLIDAR] Failed to turn on the Lidar, because the lidar is [%s].\n", + error("Failed to turn on the Lidar, because the lidar is [%s].", DriverInterface::DescribeDriverError(lidarPtr->getDriverError())); - scanning = false; return false; } - printf("[YDLIDAR] Successed to check the lidar, Elapsed time %u ms\n", getms() - t); - fflush(stdout); - - //禁用此处获取设备信息方式 - // if (m_SingleChannel && !isNetTOFLidar(m_LidarType)) - // { - // handleSingleChannelDevice(); - // } - // else - // { - // printf("[YDLIDAR] Current Sampling Rate : %.02fK\n", m_SampleRate); - // } + info("Successed to check the lidar, Elapsed time %u ms", getms() - t); m_field_of_view = 360.f; - + //网络TOF雷达需要设置视场角 if (isNetTOFLidar(m_LidarType)) { lidarConfig cfg = lidarPtr->getFinishedScanCfg(); m_field_of_view = cfg.fov_end - cfg.fov_start; - if (cfg.fov_end - 180 < m_MaxAngle) { m_MaxAngle = cfg.fov_end - 180; } - if (cfg.fov_start - 180 > m_MinAngle) { m_MinAngle = cfg.fov_start - 180; @@ -538,10 +509,11 @@ bool CYdLidar::turnOn() m_AllNode = 0; m_PointTime = lidarPtr->getPointTime(); lidarPtr->setAutoReconnect(m_AutoReconnect); - info("[YDLIDAR] Now lidar is scanning..."); + info("Now lidar is scanning..."); lastStamp = 0; - scanning = true; + //重置错误 + lidarPtr->setDriverError(NoError); return true; } @@ -740,7 +712,7 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) { scanfrequency = global_nodes[i].scanFreq / 10.0; - if (isTOFLidar(m_LidarType)) //TOF雷达转速偏移3HZ + if (isTOFLidar(m_LidarType)) //TOF雷达转速偏移3Hz { if (!isOldVersionTOFLidar(lidar_model, Major, Minjor)) { @@ -748,9 +720,10 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) } } else if (isTEALidar(lidar_model) || - isGSLidar(m_LidarType)) //TEA雷达转速范围10~30，无缩放 + isGSLidar(m_LidarType) || + isTIALidar(m_LidarType)) //TEA雷达转速范围10~30，无缩放 { - scanfrequency = global_nodes[i].scanFreq; + scanfrequency = global_nodes[i].scanFreq; } } @@ -844,16 +817,11 @@ bool CYdLidar::turnOff() { if (lidarPtr) { + if (lidarPtr->isscanning()) + info("Now lidar scanning has stopped!"); lidarPtr->stop(); } - if (scanning) - { - printf("[YDLIDAR] Now lidar scanning has stopped!\n"); - fflush(stdout); - } - - scanning = false; return true; } @@ -866,8 +834,6 @@ void CYdLidar::disconnecting() { lidarPtr->disconnect(); } - - scanning = false; } /*------------------------------------------------------------- @@ -938,7 +904,7 @@ bool CYdLidar::getUserVersion(std::string &version) { if (!checkHardware()) { - printf("[YDLIDAR] Device is not open!\n"); + error("Device is not open!"); return false; } @@ -1193,8 +1159,8 @@ bool CYdLidar::checkLidarAbnormal() int total = accumulate(data.begin(), data.end(), 0); int mean = total / data.size(); // mean value m_FixedSize = (static_cast((mean + 5) / 10)) * 10; - printf("[YDLIDAR] Single Fixed Size: %d\n", m_FixedSize); - printf("[YDLIDAR] Sample Rate: %.02fK\n", m_SampleRate); + info("Single Fixed Size: %d", m_FixedSize); + info("Sample Rate: %.02fK", m_SampleRate); return true; } @@ -1277,8 +1243,6 @@ bool CYdLidar::calcSampleRate(int count, double scan_time) } } - // printf("[YDLIDAR] Calc Sample Rate: %.2fK\n", sr); - size_t size = defalutSampleRate.size(); if (size) { @@ -1286,7 +1250,6 @@ bool CYdLidar::calcSampleRate(int count, double scan_time) { sr = defalutSampleRate.front(); ret = true; - // printf("[YDLIDAR] Calc Sample Rate1: %dK\n", sr); } else { @@ -1309,12 +1272,10 @@ bool CYdLidar::calcSampleRate(int count, double scan_time) } } ret = true; - // printf("[YDLIDAR] Calc Sample Rate2: %dK\n", sr); } } else { - // printf("[YDLIDAR] Calc Sample Rate3: %dK\n", sr); if (sr > 0) SampleRateMap[sr * 1000] ++; //放大1000倍存入 if (isValidSampleRate(SampleRateMap)) @@ -1330,10 +1291,10 @@ bool CYdLidar::calcSampleRate(int count, double scan_time) // m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); //不知转速为何要减少0.1 m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency); - printf("[YDLIDAR] Scan Frequency: %.02fHz\n", m_ScanFrequency); + info("Scan Frequency: %.02fHz", m_ScanFrequency); if (!isSDMLidar(m_LidarType)) //非SDM雷达才打印Fixed Size - printf("[YDLIDAR] Fixed Size: %d\n", m_FixedSize); - printf("[YDLIDAR] Sample Rate: %.02fK\n", m_SampleRate); + info("Fixed Size: %d", m_FixedSize); + info("Sample Rate: %.02fK", m_SampleRate); } return ret; @@ -1349,22 +1310,21 @@ bool CYdLidar::getDeviceHealth() return false; } - result_t op_result; + result_t ret; device_health healthinfo; memset(&healthinfo, 0, sizeof(device_health)); - op_result = lidarPtr->getHealth(healthinfo, - DriverInterface::DEFAULT_TIMEOUT / 2); + ret = lidarPtr->getHealth(healthinfo, + DriverInterface::DEFAULT_TIMEOUT / 2); - if (IS_OK(op_result)) + if (IS_OK(ret)) { - printf("[YDLIDAR] Lidar running correctly! The health status: %s\n", - (int)healthinfo.status == 0 ? "good" : "bad"); - + info("Lidar running correctly! The health status %s", + healthinfo.status == 0 ? "good" : "bad"); if (healthinfo.status == 2) { - fprintf(stderr, - "[YDLIDAR] Error, YDLidar internal error[0x%X] detected. " - "Please reboot the device to retry.\n", healthinfo.error_code); + error("Error, Lidar internal error[0x%X] detected. " + "Please reboot the device to retry.", + healthinfo.error_code); return false; } else @@ -1374,7 +1334,7 @@ bool CYdLidar::getDeviceHealth() } else { - fprintf(stderr, "[YDLIDAR] Error, cannot retrieve YDLidar health code: %x\n", op_result); + error("Error, cannot retrieve Lidar health code %d", ret); return false; } } @@ -1394,13 +1354,13 @@ bool CYdLidar::getDeviceInfo() DriverInterface::DEFAULT_TIMEOUT / 2); if (!IS_OK(op_result)) { - fprintf(stderr, "[YDLIDAR] Fail to get baseplate device information\n"); + error("Fail to get baseplate device information!"); return false; } if (!isSupportLidar(di.model)) { - printf("[YDLIDAR] Current SDK does not support current lidar models[%s]\n", + error("Current SDK does not support current lidar model [%s]", lidarModelToString(di.model).c_str()); return false; } @@ -1410,7 +1370,7 @@ bool CYdLidar::getDeviceInfo() { if (!isTOFLidar(m_LidarType)) { - fprintf(stderr, "Incorrect Lidar Type setting...\n"); + error("Incorrect Lidar Type setting..."); m_LidarType = TYPE_TOF; lidarPtr->setLidarType(m_LidarType); } @@ -1421,7 +1381,7 @@ bool CYdLidar::getDeviceInfo() // !isNetTOFLidarByModel(devinfo.model) && // !m_SingleChannel) // { - // fprintf(stderr, "Incorrect Lidar Type setting, Reset Type to %d...\n", + // error("Incorrect Lidar Type setting, Reset Type to %d...\n", // TYPE_TRIANGLE); // m_LidarType = TYPE_TRIANGLE; // lidarPtr->setLidarType(m_LidarType); @@ -1430,7 +1390,7 @@ bool CYdLidar::getDeviceInfo() frequencyOffset = 0.4; lidar_model = di.model; - printf("Current Lidar Model Code %d\n", lidar_model); + info("Current Lidar Model Code %d", lidar_model); // bool intensity = hasIntensity(di.model); // intensity = m_Intensity; // lidarPtr->setIntensities(intensity); @@ -1545,7 +1505,7 @@ void CYdLidar::handleSingleChannelDevice() lidar_model = di.model; // defalutSampleRate = getDefaultSampleRate(devinfo.model); - printf("[YDLIDAR] Single Channel Current Sampling Rate: %.02fK\n", m_SampleRate); + info("Single channel current sampling rate: %.02fK", m_SampleRate); return; } @@ -1558,31 +1518,25 @@ void CYdLidar::checkSampleRate() int sr = 0; int try_count = 0; m_FixedSize = 1440; - result_t ans = lidarPtr->getSamplingRate(_rate); - - if (IS_OK(ans)) + result_t ret = lidarPtr->getSamplingRate(_rate); + if (IS_OK(ret)) { - printf("[YDLIDAR] Get origin sample rate code: %u\n", _rate.rate); + info("Origin sample rate code: %u", _rate.rate); if (!isTOFLidarByModel(lidar_model)) { //非TG系列雷达获取采样率码转成采样率值 sr = ConvertUserToLidarSmaple(lidar_model, m_SampleRate, _rate.rate); - // printf("[YDLIDAR] Get sample rate code: %dK\n", sr); - //非TG系列雷达通过设备信息获取 while (sr != _rate.rate) { - ans = lidarPtr->setSamplingRate(_rate); + ret = lidarPtr->setSamplingRate(_rate); try_count++; - if (try_count > 3) { break; } } - sr = ConvertLidarToUserSmaple(lidar_model, _rate.rate); - // printf("[YDLIDAR] Get sample rate: %dK\n", sr); } else { @@ -1593,7 +1547,7 @@ void CYdLidar::checkSampleRate() m_SampleRate = sr; defalutSampleRate.clear(); defalutSampleRate.push_back(m_SampleRate); - printf("[YDLIDAR] Get sample rate: %.02fK\n", m_SampleRate); + info("Current sample rate: %.02fK", m_SampleRate); } } @@ -1615,7 +1569,7 @@ bool CYdLidar::checkScanFrequency() if (isTOFLidar(m_LidarType)) //TG雷达转速虚高0.4需要减去还原真实转速 frequency -= 0.4; hz = m_ScanFrequency - frequency; - printf("[YDLIDAR] Current scan frequency: %.02fHz\n", frequency); + info("Current scan frequency: %.02fHz", frequency); if (hz > 0) { //大调速 @@ -1654,8 +1608,8 @@ bool CYdLidar::checkScanFrequency() else { // m_ScanFrequency += frequencyOffset; - fprintf(stderr, "current scan frequency[%f] is out of range.\n", - m_ScanFrequency); + error("Current scan frequency[%f] is out of range.", + m_ScanFrequency); } ans = lidarPtr->getScanFrequency(_scan_frequency); @@ -1676,8 +1630,8 @@ bool CYdLidar::checkScanFrequency() // m_ScanFrequency -= frequencyOffset; m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); - printf("[YDLIDAR] Current scan frequency: %.02fHz\n", m_ScanFrequency); - // printf("[YDLIDAR] Fixed size: %d\n", m_FixedSize); + info("Current scan frequency: %.02fHz", m_ScanFrequency); + // info("Fixed size: %d", m_FixedSize); return true; } @@ -1743,23 +1697,24 @@ bool CYdLidar::checkCalibrationAngle(const std::string &serialNumber) m_isAngleOffsetCorrected = (angle.angle != 180 * zero_offset_angle_scale); m_AngleOffset = angle.angle / zero_offset_angle_scale; ret = true; - printf("[YDLIDAR] Successfully obtained the %s offset angle[%f] from the lidar[%s]\n", m_isAngleOffsetCorrected ? "corrected" : "uncorrrected", m_AngleOffset, - serialNumber.c_str()); + info("Successfully obtained the %s offset angle[%f] from the lidar[%s]", + m_isAngleOffsetCorrected ? "corrected" : "uncorrrected", m_AngleOffset, + serialNumber.c_str()); return ret; } retry++; } - printf("[YDLIDAR] Current %s AngleOffset : %f°\n", - m_isAngleOffsetCorrected ? "corrected" : "uncorrrected", m_AngleOffset); + info("Current %s AngleOffset : %f°", + m_isAngleOffsetCorrected ? "corrected" : "uncorrrected", m_AngleOffset); return ret; } /*------------------------------------------------------------- - checkCOMMs + checkConnect -------------------------------------------------------------*/ -bool CYdLidar::checkCOMMs() +bool CYdLidar::checkConnect() { //如果雷达类型有变化则需要先删除旧对象 if (lidarPtr && @@ -1771,7 +1726,7 @@ bool CYdLidar::checkCOMMs() //如果未创建对象 if (!lidarPtr) { - printf("[YDLIDAR] SDK initializing\n"); + info("SDK initializing"); //根据雷达类型创建对应的实例 if (isNetTOFLidar(m_LidarType)) @@ -1782,18 +1737,19 @@ bool CYdLidar::checkCOMMs() lidarPtr = new ydlidar::SDMLidarDriver(); else if (isDTSLidar(m_LidarType)) //SDM lidarPtr = new ydlidar::DTSLidarDriver(); + else if (isTIALidar(m_LidarType)) + lidarPtr = new ydlidar::TiaLidarDriver(); else //通用雷达 lidarPtr = new ydlidar::YDlidarDriver(m_DeviceType); if (!lidarPtr) { - fprintf(stderr, "[YDLIDAR] Create driver fail!\n"); + error("Create driver fail!"); return false; } - printf("[YDLIDAR] SDK has been initialized\n"); - printf("[YDLIDAR] SDK Version: %s\n", lidarPtr->getSDKVersion().c_str()); - fflush(stdout); + info("SDK has been initialized"); + info("SDK Version: %s", lidarPtr->getSDKVersion().c_str()); } if (lidarPtr->isconnected()) @@ -1805,6 +1761,7 @@ bool CYdLidar::checkCOMMs() lidarPtr->setSingleChannel(m_SingleChannel); lidarPtr->setLidarType(m_LidarType); lidarPtr->setScanFreq(m_ScanFrequency); + lidarPtr->setSampleRate(m_SampleRate); //设置采样率 lidarPtr->setSupportMotorDtrCtrl(m_SupportMotorDtrCtrl); lidarPtr->setBottom(m_Bottom); lidarPtr->setDebug(m_Debug); @@ -1836,25 +1793,23 @@ bool CYdLidar::checkCOMMs() { if (isNetTOFLidar(m_LidarType)) { - fprintf(stderr, - "[YDLIDAR] Error, cannot bind to the specified IP Address[%s]\n", - m_SerialPort.c_str()); + error("Error, cannot bind to the specified IP Address[%s]", + m_SerialPort.c_str()); } else { - fprintf(stderr, - "[YDLIDAR] Error, cannot bind to the specified [%s:%s] and [%s:%d]\n", - m_DeviceType != YDLIDAR_TYPE_SERIAL ? "IP Address" : "serial port", - m_SerialPort.c_str(), m_DeviceType != YDLIDAR_TYPE_SERIAL ? "network port" : "baudrate", m_SerialBaudrate); + error("Error, cannot bind to the specified [%s:%s] and [%s:%d]", + m_DeviceType != YDLIDAR_TYPE_SERIAL ? "IP Address" : "serial port", + m_SerialPort.c_str(), + m_DeviceType != YDLIDAR_TYPE_SERIAL ? "network port" : "baudrate", + m_SerialBaudrate); } return false; } - printf("[YDLIDAR] connect, Elapsed time %u ms\n", getms() - t); - fflush(stdout); - - printf("[YDLIDAR] Lidar successfully connected [%s:%d]\n", + info("Connect elapsed time %u ms", getms() - t); + info("Lidar successfully connected [%s:%d]", m_SerialPort.c_str(), m_SerialBaudrate); return true; } @@ -1867,8 +1822,7 @@ bool CYdLidar::checkStatus() uint32_t t = getms(); getDeviceHealth(); getDeviceInfo(); - printf("[YDLIDAR] Check status, Elapsed time %u ms\n", getms() - t); - fflush(stdout); + info("Check status, Elapsed time %u ms", getms() - t); return true; } @@ -1879,16 +1833,9 @@ bool CYdLidar::checkStatus() bool CYdLidar::checkHardware() { if (!lidarPtr) - { return false; - } - - if (scanning && lidarPtr->isscanning()) - { - return true; - } - return false; + return lidarPtr->isscanning(); } namespace ydlidar @@ -1913,4 +1860,15 @@ namespace ydlidar return ydlidar::YDlidarDriver::lidarPortList(); } +//打印logo字符 +void printLogo() +{ + info("__ ______ _ ___ ____ _ ____"); + info("\\ \\ / / _ \\| | |_ _| _ \\ / \\ | _ \\"); + info(" \\ V /| | | | | | || | | |/ _ \\ | |_) |"); + info(" | | | |_| | |___ | || |_| / ___ \\| _ <"); + info(" |_| |____/|_____|___|____/_/ \\_\\_| \\_\\"); + info(""); +} + } diff --git a/src/CYdLidar.h b/src/CYdLidar.h index 081f80d..d64025e 100644 --- a/src/CYdLidar.h +++ b/src/CYdLidar.h @@ -201,7 +201,7 @@ class YDLIDAR_API CYdLidar { * @return true if communication has been established with the device. * If it's not false on error. */ - bool checkCOMMs(); + bool checkConnect(); /** * @brief check LiDAR health state and device information * @return true if health status and device information has been obtained with the device. @@ -314,7 +314,6 @@ class YDLIDAR_API CYdLidar { bool isAngleOffsetCorrected() const; private: - bool scanning; ///< LiDAR is Scanning int m_FixedSize; ///< Fixed LiDAR Points float m_AngleOffset; ///< Zero angle offset value bool m_isAngleOffsetCorrected; ///< Has the Angle offset been corrected @@ -377,7 +376,8 @@ class YDLIDAR_API CYdLidar { #endif // CYDLIDAR_H //os -namespace ydlidar { +namespace ydlidar +{ /** * @brief system signal initialize */ @@ -391,12 +391,13 @@ YDLIDAR_API bool os_isOk(); * @brief shutdown system signal */ YDLIDAR_API void os_shutdown(); - /** * @brief lidarPortList * @return */ YDLIDAR_API std::map lidarPortList(); +//打印logo字符 +YDLIDAR_API void printLogo(); } diff --git a/src/DTSLidarDriver.cpp b/src/DTSLidarDriver.cpp index 5765ccc..f88ad7d 100644 --- a/src/DTSLidarDriver.cpp +++ b/src/DTSLidarDriver.cpp @@ -490,7 +490,7 @@ result_t DTSLidarDriver::waitPackage(node_info *node, uint32_t timeout) if (IS_OK(ret)) { - (*node).sync = Node_Sync; + (*node).sync = NODE_SYNC; (*node).stamp = getTime(); (*node).index = 0; (*node).scanFreq = uint8_t(0); @@ -560,7 +560,7 @@ result_t DTSLidarDriver::sendData(const uint8_t *data, size_t size) if (m_Debug) { printf("send: "); - printHex(data, r); + infoh(data, r); } size -= r; @@ -758,7 +758,7 @@ result_t DTSLidarDriver::getData(uint8_t *data, size_t size) if (m_Debug) { printf("recv: "); - printHex(data, r); + infoh(data, r); } //更新剩余的数据大小和数据指针 @@ -911,7 +911,7 @@ result_t DTSLidarDriver::getDeviceInfo(device_info &info, uint32_t timeout) printf("[YDLIDAR] Fail to get CalibParam\n"); return RESULT_FAIL; } - // printHex(data.data(), data.size()); + // infoh(data.data(), data.size()); //取出校准参数k,b的值 // memcpy(&k, &data[0], sizeof(float)); diff --git a/src/ETLidarDriver.cpp b/src/ETLidarDriver.cpp index e11e8b8..429e076 100644 --- a/src/ETLidarDriver.cpp +++ b/src/ETLidarDriver.cpp @@ -1043,7 +1043,7 @@ int ETLidarDriver::cacheScanData() { if (IS_OK(ans)) { timeout_count = 0; - local_scan[0].sync = Node_NotSync; + local_scan[0].sync = NODE_UNSYNC; } else { m_isScanning = false; return RESULT_FAIL; @@ -1051,7 +1051,7 @@ int ETLidarDriver::cacheScanData() { } } else { timeout_count++; - local_scan[0].sync = Node_NotSync; + local_scan[0].sync = NODE_UNSYNC; if (m_driverErrno == NoError) { setDriverError(TimeoutError); @@ -1152,7 +1152,7 @@ result_t ETLidarDriver::waitPackage(node_info *node, uint32_t timeout) { } } - (*node).sync = Node_NotSync; + (*node).sync = NODE_UNSYNC; (*node).scanFreq = 0; (*node).debugInfo = 0xff; (*node).index = 0xff; @@ -1192,7 +1192,7 @@ result_t ETLidarDriver::waitPackage(node_info *node, uint32_t timeout) { nodeIndex++; if (nodeIndex >= frame.dataNum) { - (*node).sync = frame.headFrameFlag ? Node_Sync : Node_NotSync; + (*node).sync = frame.headFrameFlag ? NODE_SYNC : NODE_UNSYNC; (*node).stamp = getTime();//(uint64_t)(frame.timestamp * 100); (*node).delayTime = 0; nodeIndex = 0; diff --git a/src/GSLidarDriver.cpp b/src/GSLidarDriver.cpp index 13e7d80..3131432 100644 --- a/src/GSLidarDriver.cpp +++ b/src/GSLidarDriver.cpp @@ -35,8 +35,9 @@ #include #include "GSLidarDriver.h" #include "core/serial/common.h" -#include -#include +#include "core/serial/serial.h" +#include "core/network/ActiveSocket.h" +#include "core/common/ydlidar_help.h" #include "ydlidar_config.h" #define GS_CMD_STARTIAP 0x0A //启动IAP @@ -325,8 +326,7 @@ result_t GSLidarDriver::sendData(const uint8_t *data, size_t size) { if (m_Debug) { - printf("send: "); - printHex(data, r); + debugh(data, r); } size -= r; @@ -352,8 +352,7 @@ result_t GSLidarDriver::getData(uint8_t *data, size_t size) { if (m_Debug) { - printf("recv: "); - printHex(data, r); + debugh(data, r); } size -= r; @@ -579,8 +578,8 @@ result_t GSLidarDriver::checkAutoConnecting() int GSLidarDriver::cacheScanData() { - node_info local_buf[GS_MAXPOINTSIZE]; - size_t count = GS_MAXPOINTSIZE; + node_info local_buf[GS_PACKMAXNODES]; + size_t count = GS_PACKMAXNODES; size_t scan_count = 0; result_t ans = RESULT_FAIL; @@ -592,7 +591,7 @@ int GSLidarDriver::cacheScanData() while (m_isScanning) { - count = GS_MAXPOINTSIZE; + count = GS_PACKMAXNODES; ans = waitScanData(local_buf, count); // Thread::needExit(); if (!IS_OK(ans)) @@ -760,7 +759,7 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout) package_Sample_Num = sample_lens + 1; // 环境2Bytes + 点云320Bytes + CRC package_recvPos = pos; nodeCount = (sample_lens - 2) / GSNODESIZE; //计算1包数据中的点数 - // printf("sample num %d\n", (package_Sample_Num - 3) / 2); + // info("Sample num %d", (package_Sample_Num - 3) / 2); pos = 0; // 解析协议数据部分 while ((waitTime = getms() - startTs) <= timeout) @@ -835,7 +834,7 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout) { model = m_models[moduleNum]; //当前雷达型号 if (m_Debug) - printf("GS Lidar Module[%d] Model[%u]\n", moduleNum, model); + debug("GS lidar module[%d] model[%u]", moduleNum, model); //根据雷达型号设置角度参数 if (YDLIDAR_GS5 == model) m_pitchAngle = Angle_PAngle2; @@ -858,7 +857,7 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout) (*node).index = moduleNum; (*node).scanFreq = m_ScanFreq; (*node).qual = 0; - (*node).sync = Node_NotSync; + (*node).sync = NODE_UNSYNC; if (YDLIDAR_GS1 == model) { @@ -945,7 +944,7 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout) else if (1 == nodeIndex) (*node).is = package.env >> 8; - // printf("%u 0x%X %.02f %.02f\n", nodeIndex, + // debug("%u 0x%X %.02f %.02f", nodeIndex, // package.nodes[nodeIndex].dist, // sampleAngle, node->dist/1.0); } @@ -962,7 +961,7 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout) if (nodeIndex >= nodeCount) { nodeIndex = 0; - (*node).sync = Node_Sync; + (*node).sync = NODE_SYNC; CheckSumResult = false; } @@ -1027,7 +1026,7 @@ void GSLidarDriver::angTransform( *dstTheta = theta; *dstDist = Dist; - // printf("%d %d %f %d\n", n, dist, (float)theta, (int)Dist); + // debug("%d %d %f %d", n, dist, (float)theta, (int)Dist); } void GSLidarDriver::angTransform2( @@ -1122,30 +1121,6 @@ result_t GSLidarDriver::grabScanData( delay(1); //延时 } return RESULT_TIMEOUT; - - // node_info packNodes[LIDAR_PACKMAXPOINTSIZE]; - // size_t packCount = 0; //单包点数 - // size_t currCount = 0; //当前点数 - // result_t ans = RESULT_FAIL; - // uint32_t st = getms(); - // uint32_t wt = 0; - // while ((wt = getms() - st) < timeout) - // { - // packCount = LIDAR_PACKMAXPOINTSIZE; - // ans = waitScanData(packNodes, packCount, timeout - wt); - // if (!IS_OK(ans)) - // { - // return ans; //失败时直接返回 - // } - // else - // { - // count = packCount; - // memcpy(nodes, packNodes, count * SDKNODESIZE); - // return RESULT_OK; - // } - // } - - // return RESULT_TIMEOUT; } result_t GSLidarDriver::ascendScanData(node_info *nodebuffer, size_t count) { @@ -1342,7 +1317,7 @@ result_t GSLidarDriver::setDeviceAddress(uint32_t timeout) return ans; } moduleCount = (h.address >> 1) + 1; - printf("[YDLIDAR] GS Lidar count %u\n", moduleCount); + info("GS lidar count %u", moduleCount); } return RESULT_OK; @@ -1455,26 +1430,14 @@ result_t GSLidarDriver::stopScan(uint32_t timeout) result_t GSLidarDriver::createThread() { - // 如果线程已启动，则先退出线程 - // if (_thread.getHandle()) - // { - // m_isScanning = false; - // _thread.join(); - // } - // _thread = CLASS_THREAD(GSLidarDriver, cacheScanData); - // if (!_thread.getHandle()) { - // return RESULT_FAIL; - // } m_thread = new std::thread(&GSLidarDriver::cacheScanData, this); if (!m_thread) { - printf("[GSLidar] Fail to create GS thread\n"); - fflush(stdout); + error("Fail to create GS thread"); return RESULT_FAIL; } - printf("[GSLidar] Create GS thread 0x%X\n", m_thread->get_id()); - fflush(stdout); + info("Create GS thread 0x%X", m_thread->get_id()); return RESULT_OK; } @@ -1597,7 +1560,7 @@ result_t GSLidarDriver::getDeviceInfo( result_t ret = setDeviceAddress(timeout); if (!IS_OK(ret)) { - printf("[YDLIDAR] Fail to get GS lidar count"); + error("Fail to get GS lidar count"); return ret; } //2、获取设备信息（带雷达型号码） @@ -1709,7 +1672,7 @@ result_t GSLidarDriver::getDeviceInfo1(device_info &info, uint32_t timeout) return ret; } -result_t GSLidarDriver::getDeviceInfo2(device_info &info, uint32_t timeout) +result_t GSLidarDriver::getDeviceInfo2(device_info &dev, uint32_t timeout) { result_t ret = RESULT_FAIL; @@ -1738,14 +1701,14 @@ result_t GSLidarDriver::getDeviceInfo2(device_info &info, uint32_t timeout) uint8_t id = uint8_t(head.address >> 1); //模组地址转编号: 1, 2, 4 m_models[id] = di.model; - printf("Get Module[%d] Lidar model[%u]\n", id, di.model); + info("Get Module[%d] Lidar model[%u]", id, di.model); if (LIDAR_MODULE_1 == head.address) { - info.model = uint8_t(di.model); - info.hardware_version = di.hwVersion; - info.firmware_version = uint16_t((di.fwVersion & 0xFF) << 8) + + dev.model = uint8_t(di.model); + dev.hardware_version = di.hwVersion; + dev.firmware_version = uint16_t((di.fwVersion & 0xFF) << 8) + uint16_t(di.fwVersion >> 8); - memcpy(info.serialnum, di.sn, SDK_SNLEN); + memcpy(dev.serialnum, di.sn, SDK_SNLEN); m_HasDeviceInfo |= EPT_Module | EPT_Base; } } @@ -1794,7 +1757,7 @@ bool GSLidarDriver::ota() { if (m_OtaName.empty()) { - printf("[YDLIDAR OTA] Not set OTA file\n"); + error("[OTA] Not set OTA file"); return false; } // 读取文件所有内容 @@ -1802,7 +1765,7 @@ bool GSLidarDriver::ota() f.open(m_OtaName, ios::in | ios::binary); if (!f.is_open()) { - printf("[YDLIDAR OTA] Fail to open OTA file[%s]\n", m_OtaName.c_str()); + error("[OTA] Fail to open OTA file[%s]", m_OtaName.c_str()); return false; } //读数据 @@ -1816,7 +1779,7 @@ bool GSLidarDriver::ota() for (int i=0; i& data) if (p != percent) { percent = p; - printf("[YDLDIAR OTA] Downloading [%d%%]\n", p); + info("[OTA] Downloading [%d%%]", p); } std::vector d; @@ -1938,7 +1901,7 @@ bool GSLidarDriver::execOta(uint8_t addr, const std::vector& data) } if (!ret) { - printf("[YDLDIAR OTA] Fail to download [%d] package\n", j + 1); + error("[OTA] Fail to download [%d] package", j + 1); break; } } diff --git a/src/SDMLidarDriver.cpp b/src/SDMLidarDriver.cpp index 843d0f5..eade47d 100644 --- a/src/SDMLidarDriver.cpp +++ b/src/SDMLidarDriver.cpp @@ -240,7 +240,7 @@ result_t SDMLidarDriver::sendData(const uint8_t *data, size_t size) if (m_Debug) { printf("send: "); - printHex(data, r); + infoh(data, r); } size -= r; @@ -268,7 +268,7 @@ result_t SDMLidarDriver::getData(uint8_t *data, size_t size) if (m_Debug) { printf("recv: "); - printHex(data, r); + infoh(data, r); } size -= r; @@ -628,7 +628,7 @@ result_t SDMLidarDriver::waitPackage(node_info *node, uint32_t timeout) if (IS_OK(ret)) { - (*node).sync = Node_Sync; + (*node).sync = NODE_SYNC; (*node).stamp = getTime(); (*node).index = 0; (*node).scanFreq = m_ScanFreq; diff --git a/src/TiaLidarDriver.cpp b/src/TiaLidarDriver.cpp new file mode 100755 index 0000000..fb5c633 --- /dev/null +++ b/src/TiaLidarDriver.cpp @@ -0,0 +1,929 @@ +#include +#include +#include "core/network/PassiveSocket.h" +#include "core/network/SimpleSocket.h" +#include "core/serial/common.h" +#include "core/common/ydlidar_help.h" +#include "TiaLidarDriver.h" + +//TIA雷达指令关键字 +#define P_TIA_SCANTYPE "scanType" //启停扫描 +#define P_TIA_SCANFREQ "target_speed_addr" //转速（寄存器值） +#define P_TIA_SAMPLERATE "measure_prf_addr" //采样率（寄存器值） +#define P_TIA_READ "Read" //读取 +#define P_TIA_CONFIG "config" //配置 +#define P_TIA_SCANFREQ2 "motorSpeed" //转速2（获取的值） +#define P_TIA_SAMPLERATE2 "samplerate" //采样率2（获取的值） +#define P_TIA_MODEL "model" //型号（获取的值） + +using namespace ydlidar; +using namespace ydlidar::core; +using namespace ydlidar::core::network; +using namespace ydlidar::core::base; +using namespace ydlidar::core::common; + + +TiaLidarDriver::TiaLidarDriver() +{ + socket_cmd = new core::network::CActiveSocket(CSimpleSocket::SocketTypeTcp); + socket_data = new core::network::CPassiveSocket(CSimpleSocket::SocketTypeUdp); + socket_data->SetSocketType(CSimpleSocket::SocketTypeUdp); + socket_cmd->SetConnectTimeout(DEFAULT_CONNECTION_TIMEOUT_SEC, + DEFAULT_CONNECTION_TIMEOUT_USEC); + scan_node_buf = new node_info[LIDAR_MAXNODES]; + scan_node_count = 0; + nodeIndex = 0; + retryCount = 0; + isAutoReconnect = true; + isAutoconnting = false; + m_Debug = false; + m_SupportMotorDtrCtrl = true; + + m_isScanning = false; + m_isConnected = false; + m_port = "192.168.0.11"; + m_baudrate = 8000; + m_SingleChannel = false; + m_LidarType = TYPE_TIA; + m_PointTime = 1e9 / 20000; +} + +TiaLidarDriver::~TiaLidarDriver() +{ + disconnect(); + + ScopedLocker l2(_lock2); + if (socket_data) { + delete socket_data; + socket_data = NULL; + } + ScopedLocker l(_lock); + if (socket_cmd) { + delete socket_cmd; + socket_cmd = NULL; + } + + if (scan_node_buf) { + delete[] scan_node_buf; + scan_node_buf = nullptr; + } +} + +result_t TiaLidarDriver::connect(const char *ip, uint32_t port) +{ + m_port = ip; + m_port2 = port; + m_isConnected = false; + + if (!configConnect(m_port.c_str(), m_port2)) + { + error("Fail to connect TCP [%s:%d],Error [%s]", + m_port.c_str(), m_port2, + DescribeError(true)); + return RESULT_FAIL; + } + info("Connect TCP [%s:%d]", m_port.c_str(), m_port2); + + if (!dataConnect(m_port.c_str(), m_baudrate)) + { + error("Fail to connect UDP [%s:%d],Error [%s]", + m_port.c_str(), m_baudrate, + DescribeError(false)); + configDisconnect(); + return RESULT_FAIL; + } + info("Connect UDP [%s:%d]", m_port.c_str(), m_baudrate); + + //设置转速 + setParam(P_TIA_SCANFREQ2, double(m_ScanFreq)); + //设置采样率 + setParam(P_TIA_SAMPLERATE2, double(m_SampleRate)); + + m_isConnected = true; + return RESULT_OK; +} + +bool TiaLidarDriver::isconnected() const { + return m_isConnected; +} + +bool TiaLidarDriver::isscanning() const { + return m_isScanning; +} + +void TiaLidarDriver::setIntensities(const int &i) { + m_intensities = i; +} + +void TiaLidarDriver::setAutoReconnect(const bool &enable) { + isAutoReconnect = enable; +} + +const char *TiaLidarDriver::DescribeError(bool isTCP) +{ + if (isTCP) + { + ScopedLocker lock(_lock); + return socket_cmd != NULL ? socket_cmd->DescribeError() : "NO Socket"; + } + else + { + ScopedLocker lock(_lock2); + return socket_data != NULL ? socket_data->DescribeError() : "NO Socket"; + } +} + +bool TiaLidarDriver::configConnect(const char *ip, int port) +{ + ScopedLocker lock(_lock); + + if (!socket_cmd) + return false; + if (!socket_cmd->IsSocketValid()) { + if (!socket_cmd->Initialize()) { + return false; + } + } else { + return socket_cmd->IsSocketValid(); + } + +// socket_cmd->SetNonblocking(); + socket_cmd->bindport(ip, port); + if (!socket_cmd->open()) +// if (!socket_cmd->Open(ip, port)) //此方式会失败 + { + socket_cmd->Close(); + return false; + } + + socket_cmd->SetReceiveTimeout( + SDK_TIMEOUT / 1000, + (SDK_TIMEOUT % 1000) * 1000); + socket_cmd->SetSendTimeout( + SDK_TIMEOUT / 1000, + (SDK_TIMEOUT % 1000) * 1000); + socket_cmd->SetBlocking(); + + return socket_cmd->IsSocketValid(); +} + +void TiaLidarDriver::configDisconnect() +{ + ScopedLocker lock(_lock); + if (!socket_cmd) + return; + if (socket_cmd->IsSocketValid()) + { + socket_cmd->Close(); + info("Disconnect TCP"); + } +} + +void TiaLidarDriver::disconnect() +{ + stopScan(); + + dataDisconnect(); + configDisconnect(); +} + +result_t TiaLidarDriver::getHealth(device_health &health, uint32_t timeout) +{ + UNUSED(timeout); + health.error_code = 0; + health.status = 0; + return RESULT_OK; +} + +result_t TiaLidarDriver::getDeviceInfo(device_info &di, uint32_t timeout) +{ + UNUSED(timeout); + + result_t ret = RESULT_FAIL; + + //设置默认值 + m_model = YDLIDAR_TIA; + + //获取设备信息 + cJSON *o = nullptr; + if (getParams(P_TIA_CONFIG, o)) + { + //雷达型号 + cJSON *jm = cJSON_GetObjectItem(o, P_TIA_MODEL); + if (cJSON_IsNumber(jm)) + { + m_model = jm->valueint; //型号码 + ret = RESULT_OK; + } + } + cJSON_Delete(o); + + di.firmware_version = 0; + di.hardware_version = 0; + di.model = m_model; + +// debug("TIA型号[%d]", m_model); + + return ret; +} + +// result_t TiaLidarDriver::start(bool force, uint32_t timeout) +// { +// UNUSED(force); +// UNUSED(timeout); + +// result_t ret = RESULT_FAIL; + +// if (m_isScanning) +// return RESULT_OK; + +// if (!IS_OK(startScan()) && +// !IS_OK(startScan())) +// return RESULT_FAIL; + +// ret = createThread(); + +// return ret; +// } + +// result_t TiaLidarDriver::stop() +// { +// //标记防止自动重连 +// if (isAutoconnting) +// isAutoconnting = false; + +// //如果未开启扫描，则直接返回 +// if (!isscanning()) +// return RESULT_OK; +// m_isScanning = false; + +// deleteThread(); + +// if (!IS_OK(stopScan()) && +// !IS_OK(stopScan())) +// return RESULT_FAIL; + +// return RESULT_OK; +// } + +bool TiaLidarDriver::createThread() +{ + m_thread = new std::thread(&TiaLidarDriver::parseScanDataThread, this); + if (!m_thread) + { + error("Fail to create data thread"); + return false; + } + info("Create TIA thread 0x%X", m_thread->get_id()); + + // m_thread2 = new std::thread(&TiaLidarDriver::parseParamInfoThread, this); + // if (!m_thread2) + // { + // warn("Fail to create TIA parameter thread"); + // } + + return true; +} + +result_t TiaLidarDriver::startScan(bool force, uint32_t timeout) +{ + UNUSED(force); + UNUSED(timeout); + + bool ret = setParam(P_TIA_SCANTYPE, 0); + + ret &= createThread(); + + return ret ? RESULT_OK : RESULT_FAIL; +} + +result_t TiaLidarDriver::stopScan(uint32_t timeout) +{ + UNUSED(timeout); + + //标记防止自动重连 + if (isAutoconnting) + isAutoconnting = false; + //如果未开启扫描，则直接返回 + if (!isscanning()) + return RESULT_OK; + m_isScanning = false; + + deleteThread(); + + //停止扫描 + bool ret = setParam(P_TIA_SCANTYPE, -1); + + return ret ? RESULT_OK : RESULT_FAIL; +} + +bool TiaLidarDriver::dataConnect(const char *ip, int port) +{ + UNUSED(ip); + + ScopedLocker l(_lock2); + if (!socket_data) + return false; + + if (!socket_data->IsSocketValid()) + { + if (socket_data->Initialize()) + { + if (!socket_data->Listen(NULL, port)) //NULL + { + socket_data->Close(); + return false; + } + + socket_data->SetReceiveTimeout(SDK_TIMEOUT / 1000, + (SDK_TIMEOUT % 1000) * 1000); + } + } + + return socket_data->IsSocketValid(); +} + +void TiaLidarDriver::dataDisconnect() +{ + ScopedLocker l(_lock2); + if (!socket_data) + return; + if (socket_data->IsSocketValid()) + { + socket_data->Close(); + info("Disconnect UDP"); + } +} + +void TiaLidarDriver::deleteThread() +{ + if (m_isScanning) + { + m_isScanning = false; + ScopedLocker l(_lock); + _dataEvent.set(); + } + if (m_thread) + { + if (m_thread->joinable()) + m_thread->join(); + delete m_thread; + m_thread = nullptr; + } + if (m_thread2) + { + if (m_thread2->joinable()) + m_thread2->join(); + delete m_thread2; + m_thread2 = nullptr; + } +} + +result_t TiaLidarDriver::grabScanData( + node_info *nodebuffer, + size_t &count, + uint32_t timeout) +{ + result_t ret = RESULT_FAIL; + switch (_dataEvent.wait(timeout)) + { + case Event::EVENT_TIMEOUT: + count = 0; + return RESULT_TIMEOUT; + + case Event::EVENT_OK: + { + if (scan_node_count == 0) + { + ret = RESULT_FAIL; + } + else + { + ScopedLocker l(_lock); + size_t size_to_copy = std::min(count, scan_node_count); + memcpy(nodebuffer, scan_node_buf, size_to_copy * sizeof(node_info)); + count = size_to_copy; + scan_node_count = 0; + ret = RESULT_OK; + } + _dataEvent.set(false); //重置事件状态 + break; + } + default: + count = 0; + return RESULT_FAIL; + } + + return ret; +} + +result_t TiaLidarDriver::getScanFrequency( + scan_frequency &frequency, + uint32_t timeout) +{ + UNUSED(frequency); + UNUSED(timeout); + + result_t ans = RESULT_FAIL; + return ans; +} + +result_t TiaLidarDriver::getSamplingRate( + sampling_rate &rate, + uint32_t timeout) +{ + UNUSED(rate); + UNUSED(timeout); + return RESULT_FAIL; +} + +result_t TiaLidarDriver::setSamplingRate( + sampling_rate &rate, + uint32_t timeout) +{ + UNUSED(rate); + UNUSED(timeout); + return RESULT_FAIL; +} + +result_t TiaLidarDriver::checkAutoConnecting() +{ + result_t ans = RESULT_FAIL; + isAutoconnting = true; //标记正在重连中 + if (m_driverErrno != BlockError) + setDriverError(TimeoutError); + + while (isAutoReconnect && isscanning()) + { + //先断开连接 + { + configDisconnect(); + dataDisconnect(); + } + //重新连接 + while (isAutoconnting && + !IS_OK(connect(m_port.c_str(), m_baudrate))) + { + setDriverError(NotOpenError); + delay(300); //延时 + } + if (!isAutoconnting) + { + m_isScanning = false; + return RESULT_FAIL; + } + //重新启动雷达 + if (isconnected() && + isAutoconnting) + { + ans = startScan(); + if (IS_OK(ans)) + { + if (getDriverError() == DeviceNotFoundError) + setDriverError(NoError); + isAutoconnting = false; + return ans; + } + else + { + setDriverError(DeviceNotFoundError); + delay(300); //延时 + } + } + } + + isAutoconnting = false; + setDriverError(NoError); + return RESULT_FAIL; +} + +int TiaLidarDriver::parseScanDataThread() +{ + node_info local_scan[LIDAR_MAXNODES]; + node_info local_buf[TIA_PACKMAXNODES]; + size_t count = TIA_PACKMAXNODES; + size_t scan_count = 0; + result_t ans = RESULT_FAIL; + int timeout_count = 0; + + memset(local_scan, 0, sizeof(local_scan)); + lastZeroTime = getms(); + lastPackIndex = 0; + + m_isScanning = true; + + while (m_isScanning) + { + count = 0; + memset(local_buf, 0, sizeof(local_buf)); + ans = getScanData(local_buf, count); + if (!IS_OK(ans)) + { + if (timeout_count > DEFAULT_TIMEOUT_COUNT) + { + if (!isAutoReconnect) + { + error("Exit scanning thread!"); + m_isScanning = false; + return RESULT_FAIL; + } + else + { + ans = checkAutoConnecting(); //重连 + if (IS_OK(ans)) { + timeout_count = 0; + local_scan[0].sync = NODE_UNSYNC; + } else { + m_isScanning = false; + return RESULT_FAIL; + } + } + } + else + { + timeout_count ++; + local_scan[0].sync = NODE_UNSYNC; + error("Timout count [%d]", timeout_count); + } + } + else + { + timeout_count = 0; + + for (size_t i = 0; i < count; ++i) + { + if (NODE_SYNC == local_buf[i].sync) + { + { + ScopedLocker l(_lock); + memcpy(scan_node_buf, local_scan, scan_count * SDKNODESIZE); + scan_node_count = scan_count; + _dataEvent.set(); + } + scan_count = 0; + } + + local_scan[scan_count++] = local_buf[i]; + if (scan_count >= LIDAR_MAXNODES) + scan_count = 0; + } + } + } + + m_isScanning = false; + + return RESULT_OK; +} + +int TiaLidarDriver::parseParamInfoThread() +{ + //定时获取转速和采样率等信息 + while (m_isScanning) + { + { + cJSON *o = nullptr; + if (getParams(P_TIA_CONFIG, o)) + { + cJSON *sf = cJSON_GetObjectItem(o, P_TIA_SCANFREQ2); + if (cJSON_IsNumber(sf)) + m_param.scanFreq = sf->valuedouble / 100; + cJSON *sr = cJSON_GetObjectItem(o, P_TIA_SAMPLERATE2); + if (cJSON_IsNumber(sr)) + m_param.sampleRate = sr->valuedouble / 100; + debug("ScanFreq: %f SampleRate: %f", + m_param.scanFreq, m_param.sampleRate); + } + cJSON_Delete(o); + } + int count = 0; + while (count++ < 4 && m_isScanning) + delay(TIMEOUT_500); + } + + return RESULT_OK; +} + +bool TiaLidarDriver::setParams(const cJSON* json) +{ + bool ret = true; + if (json->child) + { + //遍历所有值 + cJSON *child = json->child; + while (child) + { + cJSON *o = cJSON_CreateObject(); + cJSON_AddItemToObject(o, child->string, child); + ret = setParam(o); + if (!ret) + break; + child = child->next; + } + } + + return ret; +} + +bool TiaLidarDriver::setParam( + const std::string &key, + const float &value) +{ + cJSON *json = cJSON_CreateObject(); + + if (P_TIA_SCANFREQ == key) //设置寄存器值 + { + //390625/设置值=转速（单位Hz） + float v = value; + if (v > 0 && v <= 200) + cJSON_AddNumberToObject(json, key.data(), int(390625 / v)); + else + warn("TIA scanning frequency [%.02fHz] setting is incorrect", v); + } + else if (P_TIA_SAMPLERATE == key) //设置寄存器值 + { + //100000000/设置值=采样率（单位K/s） + float v = value; + if (v > 0 && v <= 300) + cJSON_AddNumberToObject(json, key.data(), int(100000 / v)); + else + warn("TIA sampling rate [%.02fK/s] setting is incorrect", v); + } + else if (P_TIA_SCANFREQ2 == key) + { + //转速（单位Hz） + int v = value; + if (v > 0 && v <= 200) + cJSON_AddNumberToObject(json, key.data(), v); + else + warn("TIA scanning frequency [%dHz] setting is incorrect", v); + } + else if (P_TIA_SAMPLERATE2 == key) + { + //100000000/设置值=采样率（单位K/s） + int v = value; + if (v > 0 && v <= 300) + cJSON_AddNumberToObject(json, key.data(), v); + else + warn("TIA sampling rate [%dK/s] setting is incorrect", v); + } + else + { + cJSON_AddNumberToObject(json, key.data(), value); + } + + return setParam(json); +} + +bool TiaLidarDriver::setParam(cJSON *json) +{ + if (!sendData(json)) + return false; + + cJSON *js = nullptr; + if (!waitResp(js, TIMEOUT_500)) + return false; + bool ret = cJSON_Compare(json, js, cJSON_bool(0)); + cJSON_Delete(js); //释放内存 + if (!ret) + { + warn("Fail to set lidar parameter,Error to response"); + return false; + } + + return true; +} + +bool TiaLidarDriver::getParams(const std::string &key, cJSON* &value) +{ + //组装发送数据 + cJSON* json = cJSON_CreateObject(); + cJSON_AddStringToObject(json, P_TIA_READ, key.data()); + if (!sendData(json)) + return false; + + if (!waitResp(value, TIMEOUT_1S)) + return false; + return true; +} + +bool TiaLidarDriver::sendData(cJSON* json) +{ + if (!json) + return false; + char* ss = cJSON_PrintUnformatted(json); //发送命令需要使用紧凑json格式 + std::string sd(ss); + free(ss); + cJSON_Delete(json); //释放内存 + //发送数据 + ScopedLocker l(_lock); + int32_t rs = socket_cmd->Send( + reinterpret_cast(sd.data()), sd.size()); + if (m_Debug) + debug("[SEND] %s", sd.c_str()); + if (rs <= 0) + { + warn("Fail to send TCP data"); + return false; + } + + return true; +} + +bool TiaLidarDriver::waitResp(cJSON* &json, uint32_t timeout) +{ + char buff[300] = {0}; + ScopedLocker l(_lock); + if (socket_cmd->Select(timeout / 1000, timeout * 1000)) + { + int32_t rs = socket_cmd->Receive(sizeof(buff), + reinterpret_cast(buff)); + if (rs > 0) //有响应数据时 + { + std::string rd(buff, rs); + if (m_Debug) + debug("[RECV] %s", rd.c_str()); + //解析JSON数据 + json = cJSON_Parse(rd.data()); + if (!json) + { + warn("Fail to wait response,Error json format"); + return false; + } + return true; + } + } + + warn("Waiting for response timeout [%u]ms", timeout); + return false; +} + +bool TiaLidarDriver::correct(float &angle, float &dist) +{ + return m_param.left.correct(angle, dist) || + m_param.right.correct(angle, dist); +} + +result_t TiaLidarDriver::getScanData(node_info* nodes, size_t& count) +{ + int32_t rs = 0; + memset(m_buff, 0, TIA_PACKMAXBUFFS); + { + ScopedLocker l(_lock2); + if (!socket_data->IsSocketValid()) + return RESULT_FAIL; + rs = socket_data->Receive(sizeof(m_buff), + reinterpret_cast(m_buff)); + if (rs <= 0) + { + warn("Fail to recv UDP data"); + return RESULT_TIMEOUT; + } + else if (rs < TIA_PACKMAXBUFFS) + { + warn("The packet length [%d] < the expected value [%d]", + rs, TIA_PACKMAXBUFFS); + return RESULT_FAIL; + } + } + uint8_t* data = m_buff; + if (m_Debug) + debugh(m_buff, rs); + + uint8_t packIndex = 0; //当前包序号 + uint64_t stamp = 0; //时间戳 + int idx = 0; //数据索引 + //解析时间戳和包序号 + idx = TIA_PACKMAXBUFFS - 8; + if (YDLIDAR_TIA_H == m_model || + YDLIDAR_TIA_X == m_model) + { + if (rs >= TIA_PACKMAXBUFFS) + { + stamp = getBigValue(&data[idx], 4); //时间戳 + idx += 4; + packIndex = getBigValue(&data[idx], 1); //包序号 + idx += 1; + } + } + else //YDLIDAR_TIA == m_model + { + if (rs >= TIA_PACKMAXBUFFS2) + { + stamp = getBigValue(&data[idx], 4) * 1000; //时间戳（秒） + idx += 4; + stamp += getBigValue(&data[idx], 4); //时间戳（毫秒） + stamp *= 1000000; //毫秒转纳秒 + idx += 4; + packIndex = uint8_t(getBigValue(&data[idx], 1) & 0x0F); //1字节低4位是包序号 + idx += 1; + } + } + if (abs(packIndex - lastPackIndex) > 1 && + packIndex != 0 && + lastPackIndex != 0) + warn("Data packet lost,current packet index [%u],last packet index [%u]", + packIndex, lastPackIndex); + lastPackIndex = packIndex; + + //大端序 + uint16_t h = 0; //头 + uint16_t a = 0; //角度 + uint8_t as = 0; //角度增量 + float sa = .0; + float ea = .0; + uint8_t p = 0; //强度 + uint16_t d = 0; //距离 + int index = 0; //点序号 + bool invalid = true; //无效标记 + idx = 0; //重置位置 + for (int i=0; i 1.0 * 100.0) + { + nodes[index].sync = NODE_SYNC; + //计算转速 +// nodes[index].scanFreq = 1e9 / (getTime() - lastZeroTime); + //赋值获取到的实际转速 + nodes[index].scanFreq = m_ScanFreq; //m_param.scanFreq; + lastZeroTime = getTime(); + if (m_Debug) + debug("Zero time [%llu]ns", lastZeroTime); + } + else + { + nodes[index].sync = NODE_UNSYNC; + } + //保留当前角度值 + m_lastAngle = a; + + //赋值 + nodes[index].index = 0; + nodes[index].is = (as >> 6); + nodes[index].angle = float(a) / 100.0 * 128; //角度值放大64倍以便和其它雷达保持一致 + nodes[index].dist = d; + nodes[index].qual = p; + + //如果是TIA-X，需要根据反射镜参数对指定角度范围内的角度和距离进行修正 + if (YDLIDAR_TIA_X == m_model) + { + float angle = a / 100.0; + float dist = d; + if (correct(angle, dist)) + { + nodes[index].angle = angle * 128; + nodes[index].dist = dist; + +// SeLog::debug("点%d 原[%.02f,%.0f] 改[%.02f,%.0f]", +// index, a / 100.0, float(d), angle, dist); + } + } + +// SeLog::debug("点%d [a:%.02f+%.02f d:%u i:%u]", +// index, a / 100.0, (as && 0x3F) / 100.0, d, p); + + index ++; + + //如果是无效数据，需要终止本次解析 + if (invalid) + break; + } + if (invalid) + break; + } + if (m_Debug) + debug("Angle range [%.02f~%.02f] stamp [%lu]ns index [%u]", + sa, ea, stamp, packIndex); + + count = index; + + return RESULT_OK; +} \ No newline at end of file diff --git a/src/TiaLidarDriver.h b/src/TiaLidarDriver.h new file mode 100755 index 0000000..4c8e93d --- /dev/null +++ b/src/TiaLidarDriver.h @@ -0,0 +1,248 @@ +#pragma once +#include +#include +#include "core/base/thread.h" +#include "core/base/locker.h" +#include "core/common/ydlidar_protocol.h" +#include "core/common/ydlidar_datatype.h" +#include "core/common/DriverInterface.h" +#include "core/json/cJSON.h" + + +//雷达参数 +struct EaiLidarBaseParam +{ + int type = 0; //雷达类型 +}; +//TIA雷达参数 +struct EaiTiaParam : public EaiLidarBaseParam +{ + std::string ip = "192.168.0.11"; //IP地址 + int port = 8090; //端口号 + std::string mask = "255.255.255.0"; //子网掩码 + std::string gateway = "192.168.0.1"; //网关 + std::string mac; //MAC地址 + + float scanFreq = .0f; //扫描频率，单位Hz + float sampleRate = .0f; //采样率，单位K/s +}; +//TIA-X雷达参数 +struct EaiTiaXParamItem //项 +{ + float angle = 45.0f; //反射镜安装角度（单位°） + float dist = 30.0f; //反射镜安装距离（单位mm） + float opd = 0.0f; //光程差（单位mm） + float minAngle = 60.0f; //反射镜起始角度（单位°） + float maxAngle = 100.0f; //反射镜结束角度（单位°） + + //根据参数修正角度和距离 + bool correct(float &angle, float &dist) const { + //距离无效直接返回 + if (int(dist) <= 0) + return false; + bool has = angle > SDK_ANGLE180; //超过180°的标记 + //先判断角度值是否有效 + if (angle >= minAngle && + angle <= maxAngle) //左反射镜 + { + if (has) + angle = SDK_ANGLE360 - angle; + float beta = SDK_ANGLE180 - this->angle - angle; + float a = this->dist / sin(beta * M_PI / SDK_ANGLE180); //边长a + float b = dist - a; //边长b + float C = (SDK_ANGLE180 - 2 * beta) * M_PI / SDK_ANGLE180; //角C（弧度） + float c = sqrt(a * a + b * b - 2 * a * b * cos(C)); //边长c + //余弦定理计算 + float B = acos((a * a + c * c - b * b) / (2 * a * c)) * + SDK_ANGLE180 / M_PI; //角B（角度） + if (has) + angle = SDK_ANGLE360 - (B + angle); + else + angle += B; + + dist = c + this->opd; //距离增加光程差值 + + return true; + } + return false; + } +}; +struct EaiTiaXParam : public EaiTiaParam +{ + EaiTiaXParamItem left; //左反射镜参数 + EaiTiaXParamItem right; //右反射镜参数 +}; + +namespace ydlidar { +namespace core { +namespace network { +class CActiveSocket; +class CPassiveSocket; +} +} + +using namespace core::common; +using namespace core::base; +using namespace core::network; + +class TiaLidarDriver : public DriverInterface +{ +public: + explicit TiaLidarDriver(); + ~TiaLidarDriver(); + + //连接雷达 + virtual result_t connect(const char *port_path, uint32_t baudrate = 8000); + //错误描述 + virtual const char *DescribeError(bool isTCP = true); + //断连雷达 + virtual void disconnect(); + //是否在扫描状态 + virtual bool isscanning() const; + //是否在连接状态 + virtual bool isconnected() const; + + /*! + * @brief 设置雷达是否带信号质量 \n + * 连接成功后，必须使用::disconnect函数关闭 + * @param[in] isintensities 是否带信号质量: + * true 带信号质量 + * false 无信号质量 + * @note只有S4B(波特率是153600)雷达支持带信号质量, 别的型号雷达暂不支持 + */ + virtual void setIntensities(const int &i); + + /*! + * @brief 设置雷达异常自动重新连接 \n + * @param[in] enable 是否开启自动重连: + * true 开启 + * false 关闭 + */ + virtual void setAutoReconnect(const bool &enable); + //获取雷达设备健康状态 + virtual result_t getHealth( + device_health &health, + uint32_t timeout = SDK_TIMEOUT); + //获取设备信息 + virtual result_t getDeviceInfo( + device_info &info, + uint32_t timeout = SDK_TIMEOUT); + // //启动雷达 + // virtual result_t start( + // bool force = false, + // uint32_t timeout = SDK_TIMEOUT); + // //停止雷达 + // virtual result_t stop(); + /*! + * @brief 获取激光数据 \n + * @param[in] nodebuffer 激光点信息 + * @param[in] count 一圈激光点数 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_FAILE 获取失败 + * @note 获取之前，必须使用::startScan函数开启扫描 + */ + virtual result_t grabScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout = SDK_TIMEOUT) ; + /*! + * @brief 获取激光雷达当前扫描频率 \n + * @param[in] frequency 扫描频率 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 停止扫描后再执行当前操作 + */ + virtual result_t getScanFrequency(scan_frequency &frequency, + uint32_t timeout = SDK_TIMEOUT); + + /*! + * @brief 获取激光雷达当前采样频率 \n + * @param[in] frequency 采样频率 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 停止扫描后再执行当前操作 + */ + virtual result_t getSamplingRate( + sampling_rate &rate, + uint32_t timeout = SDK_TIMEOUT); + + /*! + * @brief 设置激光雷达当前采样频率 \n + * @param[in] rate 　　　采样频率 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 停止扫描后再执行当前操作 + */ + virtual result_t setSamplingRate( + sampling_rate &rate, + uint32_t timeout = SDK_TIMEOUT); + +private: + //连接TCP + bool configConnect(const char *lidarIP, int tcpPort = 8090); + //断连TCP + void configDisconnect(); + //连接UDP + bool dataConnect(const char *lidarIP, int localPort = 8000); + //断连UDP + void dataDisconnect(); + //启动扫描 + virtual result_t startScan( + bool force = false, + uint32_t timeout = SDK_TIMEOUT); + //停止扫描 + virtual result_t stopScan( + uint32_t timeout = SDK_TIMEOUT); + //创建线程 + bool createThread(); + //销毁线程 + void deleteThread(); + + result_t getScanData(node_info* nodes, size_t& count); + //检查自动连接 + result_t checkAutoConnecting(); + + //解析扫描数据线程函数 + int parseScanDataThread(); + //定时获取转速等信息线程函数 + int parseParamInfoThread(); + + //设置多个参数 + bool setParams(const cJSON* json); + //设置单个参数 + bool setParam(const std::string& key, const float& value); + bool setParam(cJSON* json); + //获取多个参数 + bool getParams(const std::string& key, cJSON* &value); + //发送数据 + bool sendData(cJSON* json); + //等待响应 + bool waitResp(cJSON* &json, uint32_t timeout = SDK_TIMEOUT); + +private: + //TIA-X修正角度和距离 + bool correct(float& a, float& d); + +private: + int m_model = YDLIDAR_TIA; //雷达型号 + float m_lastAngle = 0.f; + int m_port2 = 9000; + /* Sockets for ydlidar */ + CActiveSocket *socket_cmd = nullptr; + CPassiveSocket *socket_data = nullptr; + uint8_t m_buff[TIA_PACKMAXBUFFS2]; //缓存 + uint64_t lastZeroTime = 0; //上一零位点时间 + uint8_t lastPackIndex = 0; //上一包包序号 + //TIA-X专用参数 + EaiTiaXParam m_param; //参数 + Thread _thread2; //参数线程 + Locker _lock2; //操作串口或网络的锁（不支持嵌套） +}; + +} //ydlidar diff --git a/src/YDlidarDriver.cpp b/src/YDlidarDriver.cpp index 523df9b..23348fa 100644 --- a/src/YDlidarDriver.cpp +++ b/src/YDlidarDriver.cpp @@ -21,12 +21,13 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. // -#include -#include -#include "YDlidarDriver.h" -#include #include -#include +#include "core/common/ydlidar_help.h" +#include "core/serial/common.h" +#include "core/serial/serial.h" +#include "core/network/ActiveSocket.h" +#include "YDlidarDriver.h" +#include "ydlidar_config.h" using namespace impl; @@ -161,7 +162,7 @@ namespace ydlidar //如果是双通雷达，需要先停止 if (!m_SingleChannel) { - printf("[YDLIDAR] Stop Lidar\n"); + info("Stop Lidar"); stop(); } @@ -338,8 +339,7 @@ namespace ydlidar if (m_Debug) { - printf("send: "); - printHex(data, r); + debugh(data, r); } size -= r; @@ -365,8 +365,7 @@ namespace ydlidar if (m_Debug) { - printf("recv: "); - printHex(data, r); + debugh(data, r); } size -= r; @@ -631,20 +630,14 @@ namespace ydlidar { count = 128; ans = waitScanData(local_buf, count, DEFAULT_TIMEOUT / 2); - - // Thread::needExit(); - if (!IS_OK(ans)) { if (timeout_count > DEFAULT_TIMEOUT_COUNT) { if (!isAutoReconnect) { - fprintf(stderr, "[YDLIDAR] Exit scanning thread\n"); - fflush(stderr); - { - m_isScanning = false; - } + error("Exit scanning thread!"); + m_isScanning = false; return RESULT_FAIL; } else @@ -659,7 +652,7 @@ namespace ydlidar if (IS_OK(ans)) { timeout_count = 0; - local_scan[0].sync = Node_NotSync; + local_scan[0].sync = NODE_UNSYNC; } else { @@ -671,13 +664,12 @@ namespace ydlidar else { timeout_count++; - local_scan[0].sync = Node_NotSync; + local_scan[0].sync = NODE_UNSYNC; if (m_driverErrno == NoError) setDriverError(TimeoutError); - fprintf(stderr, "[YDLIDAR] Timeout count: %d\n", timeout_count); - fflush(stderr); + error("Timeout count: %d", timeout_count); } } else @@ -698,22 +690,19 @@ namespace ydlidar { if (local_buf[pos].sync & LIDAR_RESP_SYNCBIT) { - // printf("[YDLIDAR] S2 points Stored in buffer start %lu\n", scan_count); if (local_scan[0].sync & LIDAR_RESP_SYNCBIT) { ScopedLocker l(_lock); local_scan[0].delayTime = local_buf[pos].delayTime; - // 将下一圈的第一个点的采集时间作为当前圈数据的采集时间 + //TODO: 将下一圈的第一个点的采集时间作为当前圈数据的采集时间 memcpy(scan_node_buf, local_scan, scan_count * sizeof(node_info)); scan_node_count = scan_count; _dataEvent.set(); - // printf("[YDLIDAR] S2 points Stored in buffer end %lu\n", scan_count); } scan_count = 0; } - local_scan[scan_count++] = local_buf[pos]; if (scan_count == _countof(local_scan)) @@ -823,9 +812,6 @@ namespace ydlidar for (size_t pos = 0; pos < recvSize; ++pos) { uint8_t currentByte = globalRecvBuffer[pos]; - - // printf("c:%02X p:%d\n", currentByte, recvPos); - switch (recvPos) { case 0: @@ -858,7 +844,6 @@ namespace ydlidar int remainSize = SIZE_STAMPPACKAGE - (recvSize - pos + 1); // 计算剩余应读字节数 if (remainSize > 0) { - // printf("remainSize: %u\n", remainSize); size_t lastSize = recvSize; ans = waitForData(remainSize, timeout - waitTime, &recvSize); if (!IS_OK(ans)) @@ -885,16 +870,14 @@ namespace ydlidar } if (csc != csr) { - printf("[YDLIDAR] Checksum error c[0x%02X] != r[0x%02X]\n", csc, csr); - fflush(stdout); + error("Checksum error c[0x%02X] != r[0x%02X]", csc, csr); } else { stamp_package sp; memcpy(&sp, &globalRecvBuffer[lastPos], SIZE_STAMPPACKAGE); stamp = uint64_t(sp.stamp) * 1000000; // 毫秒转纳秒需要×1000000 - // printf("stamp: 0x%"PRIx64" -> 0x%"PRIx64"\n", sp.stamp, stamp); - // fflush(stdout); + // debug("stamp: 0x%"PRIx64" -> 0x%"PRIx64"", sp.stamp, stamp); // 测试扫描时长 // static uint32_t s_scanTime = 0; // if (s_scanTime > 0) @@ -902,8 +885,8 @@ namespace ydlidar // uint32_t dt = sp.stamp - s_scanTime; // if (dt < 44 || dt > 57) // { - // error("单帧时长[%u]ms超出标准[%u~%u]", - // dt, 44, 57); + // error("单帧时长[%u]ms超出标准[%u~%u]", + // dt, 44, 57); // } // } // s_scanTime = sp.stamp; @@ -1144,7 +1127,7 @@ namespace ydlidar if (hasStamp) { memcpy(&stamp, globalRecvBuffer, size); - // printf("SCL stamp: %llu 0x%"PRIx64"\n", stamp, stamp); + // debug("SCL stamp: %llu 0x%"PRIx64"", stamp, stamp); } return hasStamp; @@ -1220,14 +1203,14 @@ namespace ydlidar ct = packages.ct; nowPackageNum = packages.count; } - // printf("[YDLIDAR] S2 pack points %u\n", nowPackageNum); + // debug("S2 pack points %u", nowPackageNum); } void YDlidarDriver::parseNodeDebugFromBuffer(node_info *node) { if ((ct & 0x01) == CT_Normal) { - (*node).sync = Node_NotSync; + (*node).sync = NODE_UNSYNC; (*node).debugInfo = 0xff; if (!has_package_error) @@ -1248,10 +1231,10 @@ namespace ydlidar } else { - (*node).sync = Node_Sync; + (*node).sync = NODE_SYNC; package_index = 0; - // printf("start angle %f end angle %f\n", + // debug("start angle %f end angle %f", // float(FirstSampleAngle) / 64.0, // float(LastSampleAngle) / 64.0); @@ -1294,8 +1277,7 @@ namespace ydlidar package.nodes[nodeIndex].dist & 0xfffc; (*node).is = package.nodes[nodeIndex].dist & 0x0003; - // printf("%d d:%u\n", nodeIndex, (*node).dist); - // fflush(stdout); + // debug("%d d:%u", nodeIndex, (*node).dist); } else { @@ -1320,7 +1302,7 @@ namespace ydlidar if ((*node).dist != 0) { - // printf("has angle 2nd parse %d %d\n", m_LidarType, model); + // debug("has angle 2nd parse %d %d", m_LidarType, model); if (isOctaveLidar(model)) { correctAngle = (int32_t)(((atan(((21.8 * (155.3 - ((*node).dist / 2.0))) / 155.3) / ((*node).dist / 2.0))) * 180.0 / 3.1415) * 64.0); @@ -1331,7 +1313,7 @@ namespace ydlidar // SCL雷达角度二级解析公式 // correctAngle = int32_t(asin(17.8 / node->dist) * 180.0 / M_PI * 64.0); correctAngle = int32_t(atan(17.8 / (node->dist / 4.0)) * 180.0 / M_PI * 64.0); - // printf("SCL correct angle [%d]\n", correctAngle); + // debug("SCL correct angle [%d]", correctAngle); } else if (isTriangleLidar(m_LidarType) && !isTminiLidar(model) && // 去掉Tmini雷达的角度二级解析 @@ -1341,7 +1323,7 @@ namespace ydlidar } else { - // printf("no angle 2nd parse\n"); + //debug("no angle 2nd parse"); } m_InvalidNodeCount++; @@ -1381,7 +1363,7 @@ namespace ydlidar } else { - (*node).sync = Node_NotSync; + (*node).sync = NODE_UNSYNC; (*node).qual = Node_Default_Quality; (*node).angle = LIDAR_RESP_CHECKBIT; (*node).dist = 0; @@ -1436,7 +1418,7 @@ namespace ydlidar { size_t packageNum = 0; size_t Number = 0; - size_t PackageSize = TRI_PACKDATASIZE; + size_t PackageSize = TRI_PACKMAXNODES; packageNum = size / PackageSize; Number = size % PackageSize; delayTime = packageNum * (PackageSize - TRI_PACKHEADSIZE) * m_PointTime / 2; @@ -1486,50 +1468,6 @@ namespace ydlidar count = 0; return RESULT_FAIL; } - - // node_info packNodes[LIDAR_PACKMAXPOINTSIZE]; - // size_t packCount = 0; //单包点数 - // size_t currCount = 0; //当前点数 - // result_t ans = RESULT_FAIL; - // uint32_t st = getms(); - // uint32_t wt = 0; - // while ((wt = getms() - st) < timeout) - // { - // packCount = LIDAR_PACKMAXPOINTSIZE; - // ans = waitScanData(packNodes, packCount, timeout - wt); - // if (!IS_OK(ans)) - // { - // return ans; //失败时直接返回 - // } - // else - // { - // bool hasZero = false; //当前包中是否有零位标记 - // for (size_t i = 0; i < packCount; ++i) - // { - // if (packNodes[i].sync & LIDAR_RESP_SYNCBIT) - // { - // hasZero = true; - // } - - // nodes[currCount ++] = packNodes[i]; - // if (currCount >= count) - // { - // hasZero = true; - // printf("[YDLIDAR] Current points count %d > buffer size %d\n", - // currCount, count); - // fflush(stdout); - // break; - // } - // } - // if (hasZero) - // { - // count = currCount; - // return RESULT_OK; - // } - // } - // } - - // return RESULT_TIMEOUT; } result_t YDlidarDriver::ascendScanData(node_info *nodebuffer, size_t count) @@ -1992,30 +1930,14 @@ namespace ydlidar result_t YDlidarDriver::createThread() { - // //如果线程已启动，则先退出线程 - // if (_thread.getHandle()) - // { - // m_isScanning = false; - // _thread.join(); - // } - // _thread = CLASS_THREAD(YDlidarDriver, cacheScanData); - // if (!_thread.getHandle()) { - // return RESULT_FAIL; - // } - - // printf("[YDLIDAR] Create thread 0x%X\n", _thread.getHandle()); - // fflush(stdout); - // return RESULT_OK; m_thread = new std::thread(&YDlidarDriver::cacheScanData, this); if (!m_thread) { - printf("[YDLIDAR] Fail to create thread\n"); - fflush(stdout); + error("[YDLIDAR] Fail to create thread"); return RESULT_FAIL; } - printf("[YDLIDAR] Create thread 0x%X\n", m_thread->get_id()); - fflush(stdout); + info("[YDLIDAR] Create thread 0x%X", m_thread->get_id()); return RESULT_OK; } @@ -2692,20 +2614,11 @@ namespace ydlidar recvSize = remainSize; getData(s_buff, recvSize); - // ans = getData(s_buff, recvSize); - // if (!IS_OK(ans)); - // return ans; - - // printf("recv: "); - // printHex(s_buff, recvSize); - for (size_t pos = 0; pos < recvSize; ++pos) { uint8_t c = s_buff[pos]; m_dataPos++; - // printf("i %u c 0x%02X p %d\n", m_dataPos, c, recvPos); - switch (recvPos) { case 0: @@ -2790,8 +2703,7 @@ namespace ydlidar if (!isTriangleLidar(m_LidarType)) return RESULT_OK; - printf("[YDLIDAR] Start to getting intensity flag\n"); - fflush(stdout); + info("Start to getting intensity flag"); m_dataPos = 0; uint32_t lastOffset = 0; @@ -2806,8 +2718,6 @@ namespace ydlidar uint32_t lastPos = 0; // 上一包包头位置 while (IS_OK(parseHeader(zero, headPos, 500))) { - // printf("zero %u pos %u\n", zero, headPos); - // fflush(stdout); if (zero) { lastZero = 1; @@ -2819,9 +2729,6 @@ namespace ydlidar lastZero = 0; offset = headPos - lastPos; - // printf("lastPos %u currPos %u offset %u\n", lastPos, headPos, offset); - // fflush(stdout); - if (offset != ZERO_OFFSET12 && offset != ZERO_OFFSET13) break; @@ -2829,7 +2736,7 @@ namespace ydlidar if (lastOffset && lastOffset != offset) { - printf("[YDLIDAR] Fail to getting intensity\n"); + warn("Fail to getting intensity"); return RESULT_FAIL; } @@ -2853,11 +2760,10 @@ namespace ydlidar setIntensities(true); m_intensityBit = 8; } - printf("[YDLIDAR] Auto set intensity %d\n", m_intensities); + info("Auto set intensity %d", m_intensities); } - printf("[YDLIDAR] End to getting intensity flag\n"); - fflush(stdout); + info("[YDLIDAR] End to getting intensity flag"); return RESULT_OK; }