parent
3307a2e8ae
commit
097f10b44b
File diff suppressed because it is too large
Load Diff
@ -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 <stddef.h>
|
||||||
|
|
||||||
|
/* 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
|
@ -0,0 +1,23 @@
|
|||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#define EARTH_RADIUS 6378137 // 地球半径
|
||||||
|
|
||||||
|
void geodetic2enu(double latitude, double longitude, double latitude0, double longitude0, int *x, int *y) {
|
||||||
|
double flattening = 1 / 298.257223563; // 扁率
|
||||||
|
|
||||||
|
double e_square = flattening * (2.0 - flattening);
|
||||||
|
double N0 = EARTH_RADIUS / sqrt(1 - e_square * sin(latitude0) * sin(latitude0));
|
||||||
|
double X0 = N0 * cos(latitude0) * cos(longitude0);
|
||||||
|
double Y0 = N0 * cos(latitude0) * sin(longitude0);
|
||||||
|
|
||||||
|
double N = EARTH_RADIUS / sqrt(1 - e_square * sin(latitude) * sin(latitude));
|
||||||
|
double X = N * cos(latitude) * cos(longitude);
|
||||||
|
double Y = N * cos(latitude) * sin(longitude);
|
||||||
|
|
||||||
|
double deltaX = X - X0;
|
||||||
|
double deltaY = Y - Y0;
|
||||||
|
|
||||||
|
// 将浮点类型的坐标转换为整数类型
|
||||||
|
*x = (int)deltaY;
|
||||||
|
*y = (int)(-deltaX);
|
||||||
|
}
|
@ -0,0 +1 @@
|
|||||||
|
void geodetic2enu(double latitude, double longitude, double latitude0, double longitude0, int *x, int *y);
|
@ -0,0 +1,342 @@
|
|||||||
|
#include <ros/ros.h>
|
||||||
|
#include <iostream>
|
||||||
|
#include <pthread.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <prometheus_msgs/ControlCommand.h>
|
||||||
|
#include <prometheus_msgs/DroneState.h>
|
||||||
|
#include <sys/socket.h>
|
||||||
|
#include <arpa/inet.h>
|
||||||
|
#include <netinet/in.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <cstring>
|
||||||
|
#include <string>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <locale>
|
||||||
|
#include <codecvt>
|
||||||
|
#include <unicode/utypes.h>
|
||||||
|
#include <unicode/unistr.h>
|
||||||
|
#include <unicode/ustream.h>
|
||||||
|
#include "convert.h"
|
||||||
|
#include <time.h>
|
||||||
|
|
||||||
|
#include "cJSON.h"
|
||||||
|
#define NODE_NAME "terminal_control"
|
||||||
|
#define latitude_home 28.194
|
||||||
|
#define longitude_home 112.982
|
||||||
|
using namespace std;
|
||||||
|
prometheus_msgs::ControlCommand Command_to_offboard;
|
||||||
|
prometheus_msgs::ControlCommand Command_to_Move;
|
||||||
|
prometheus_msgs::ControlCommand Command_to_Return;
|
||||||
|
prometheus_msgs::ControlCommand Command_to_Return_Land;
|
||||||
|
prometheus_msgs::ControlCommand Command_to_Hold;
|
||||||
|
prometheus_msgs::ControlCommand Command_to_Takeoff;
|
||||||
|
prometheus_msgs::DroneState _DroneState;
|
||||||
|
float latitude;
|
||||||
|
float longitude;
|
||||||
|
int hours;
|
||||||
|
int minutes;
|
||||||
|
int seconds;
|
||||||
|
int arrival_flag = 0;
|
||||||
|
int hold_flag = 0;
|
||||||
|
int goal_x;
|
||||||
|
int goal_y;
|
||||||
|
int goal_z;
|
||||||
|
void Command_cb(const prometheus_msgs::ControlCommand::ConstPtr& msg){
|
||||||
|
Command_to_offboard.Command_ID = msg->Command_ID+1;
|
||||||
|
Command_to_Move.Command_ID = msg->Command_ID+1;
|
||||||
|
Command_to_Return.Command_ID = msg->Command_ID+1;
|
||||||
|
Command_to_Return_Land.Command_ID = msg->Command_ID+1;
|
||||||
|
Command_to_Hold.Command_ID = msg->Command_ID+1;
|
||||||
|
Command_to_Takeoff.Command_ID = msg->Command_ID+1;
|
||||||
|
ROS_INFO("%d",Command_to_offboard.Command_ID);
|
||||||
|
}
|
||||||
|
void Arrival_check(const prometheus_msgs::DroneState::ConstPtr& msg){
|
||||||
|
//ROS_INFO("??? go this way?");
|
||||||
|
_DroneState = *msg;
|
||||||
|
float distance = sqrt(pow(_DroneState.position[0] - Command_to_Move.Reference_State.position_ref[0], 2) +
|
||||||
|
pow(_DroneState.position[1] - Command_to_Move.Reference_State.position_ref[1], 2) +
|
||||||
|
pow(_DroneState.position[2] - Command_to_Move.Reference_State.position_ref[2], 2));
|
||||||
|
//ROS_INFO("Distance to target: %.2f", distance);
|
||||||
|
if((int)distance < 2){
|
||||||
|
arrival_flag = 1;
|
||||||
|
//ROS_INFO("Arrival_flag:%d",arrival_flag);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
class SocketClient {
|
||||||
|
private:
|
||||||
|
|
||||||
|
struct sockaddr_in server_addr;
|
||||||
|
public:
|
||||||
|
int sock;
|
||||||
|
SocketClient(const char* ip_address, int port) {
|
||||||
|
// 创建 socket
|
||||||
|
sock = socket(AF_INET, SOCK_STREAM, 0);
|
||||||
|
if (sock == -1) {
|
||||||
|
throw runtime_error("Failed to create socket!");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 初始化服务器地址
|
||||||
|
memset(&server_addr, 0, sizeof(server_addr));
|
||||||
|
server_addr.sin_family = AF_INET;
|
||||||
|
server_addr.sin_port = htons(port);
|
||||||
|
server_addr.sin_addr.s_addr = inet_addr(ip_address);
|
||||||
|
|
||||||
|
// 连接服务器
|
||||||
|
if (connect(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) == -1) {
|
||||||
|
close(sock);
|
||||||
|
throw runtime_error("Failed to connect to server!");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
~SocketClient() {
|
||||||
|
close(sock);
|
||||||
|
}
|
||||||
|
|
||||||
|
string receive(int buffer_size = 1024) {
|
||||||
|
char buffer[buffer_size];
|
||||||
|
int bytes_received = recv(sock, buffer, sizeof(buffer), 0);
|
||||||
|
if (bytes_received == -1) {
|
||||||
|
ROS_INFO("Failed to receive data from server!");
|
||||||
|
} else {
|
||||||
|
buffer[bytes_received] = '\0';
|
||||||
|
return string(buffer);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void send(const string& message) {
|
||||||
|
if (::send(sock, message.c_str(), message.length(), 0) == -1) {
|
||||||
|
throw runtime_error("Failed to send data to server!");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
SocketClient client("192.168.8.100", 9090);
|
||||||
|
void *TCP_recv(void *arg){
|
||||||
|
while(true){
|
||||||
|
std::vector<char> buffer(1024);
|
||||||
|
int n = recv(client.sock, buffer.data(), buffer.size(), 0);
|
||||||
|
std::string data;
|
||||||
|
if (n > 0) {
|
||||||
|
std::string str(buffer.data(), n); // 使用 UTF-8 解码字符串
|
||||||
|
std::cout << str << '\n';
|
||||||
|
}
|
||||||
|
|
||||||
|
ROS_INFO("%s",buffer.data());
|
||||||
|
cJSON *root = cJSON_Parse(buffer.data());
|
||||||
|
const char *type= cJSON_GetObjectItem(root, "type")->valuestring;
|
||||||
|
if(!strcmp(type,"Alarming")){
|
||||||
|
const char *fire_flag = cJSON_GetObjectItem(root, "fire_flag")->valuestring;
|
||||||
|
ROS_INFO("%s",fire_flag);
|
||||||
|
if(!strcmp(fire_flag,"fire")){
|
||||||
|
hold_flag = 1;}
|
||||||
|
else
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if(!strcmp(type,"Set")){
|
||||||
|
cJSON *latitude_r = cJSON_GetObjectItem(root, "latitude");
|
||||||
|
latitude = (float)cJSON_GetNumberValue(latitude_r);
|
||||||
|
cJSON *longitude_r = cJSON_GetObjectItem(root, "longitude");
|
||||||
|
longitude = (float)cJSON_GetNumberValue(longitude_r);
|
||||||
|
hours = cJSON_GetObjectItem(root, "hours")->valueint;
|
||||||
|
minutes = cJSON_GetObjectItem(root, "minutes")->valueint;
|
||||||
|
seconds = cJSON_GetObjectItem(root, "seconds")->valueint;
|
||||||
|
//x = cJSON_GetObjectItem(root,"x")->valueint;
|
||||||
|
//y = cJSON_GetObjectItem(root,"y")->valueint;
|
||||||
|
//z = cJSON_GetObjectItem(root,"z")->valueint;
|
||||||
|
if(latitude != 0 && longitude != 0){
|
||||||
|
//ROS_INFO("%f,%f,%f,%d,%d,%d",latitude,longitude,hours,minutes,seconds);
|
||||||
|
geodetic2enu(latitude,longitude,latitude_home, longitude_home,&goal_x,&goal_y);
|
||||||
|
ROS_INFO("x:%d,y:%d",goal_x,goal_y);
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
//goal_x = x;
|
||||||
|
//goal_y = y;
|
||||||
|
//goal_z = z;
|
||||||
|
}
|
||||||
|
} //Set Check
|
||||||
|
}//while
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char** argv)
|
||||||
|
{
|
||||||
|
// 初始化 ROS 节点
|
||||||
|
ros::init(argc, argv, "control_publisher");
|
||||||
|
ros::NodeHandle nh;
|
||||||
|
ros::Rate loop_rate(10); // 发布频率为 10 Hz
|
||||||
|
ros::Subscriber Command_sub = nh.subscribe<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10, Command_cb);
|
||||||
|
ros::Subscriber position_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, Arrival_check);
|
||||||
|
ROS_INFO("I am subing");
|
||||||
|
|
||||||
|
ros::Publisher control_offboard = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
|
||||||
|
// 创建一个发布者对象,发布 prometheus_msgs::ControlCommand 类型的消息到 /prometheus/control_command 话题
|
||||||
|
|
||||||
|
//offboard
|
||||||
|
Command_to_offboard.Mode = prometheus_msgs::ControlCommand::Idle;
|
||||||
|
Command_to_offboard.source = NODE_NAME;
|
||||||
|
Command_to_offboard.Command_ID = 1;
|
||||||
|
Command_to_offboard.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
|
||||||
|
Command_to_offboard.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
|
||||||
|
Command_to_offboard.Reference_State.position_ref[0] = 0;
|
||||||
|
Command_to_offboard.Reference_State.position_ref[1] = 0;
|
||||||
|
Command_to_offboard.Reference_State.position_ref[2] = 0;
|
||||||
|
Command_to_offboard.Reference_State.velocity_ref[0] = 0;
|
||||||
|
Command_to_offboard.Reference_State.velocity_ref[1] = 0;
|
||||||
|
Command_to_offboard.Reference_State.velocity_ref[2] = 0;
|
||||||
|
Command_to_offboard.Reference_State.acceleration_ref[0] = 0;
|
||||||
|
Command_to_offboard.Reference_State.acceleration_ref[1] = 0;
|
||||||
|
Command_to_offboard.Reference_State.acceleration_ref[2] = 0;
|
||||||
|
Command_to_offboard.Reference_State.yaw_ref = 999.0;
|
||||||
|
//Takeoff
|
||||||
|
Command_to_Takeoff.Mode = prometheus_msgs::ControlCommand::Move;
|
||||||
|
Command_to_Takeoff.source = NODE_NAME;
|
||||||
|
Command_to_Takeoff.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
|
||||||
|
Command_to_Takeoff.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
|
||||||
|
Command_to_Takeoff.Reference_State.position_ref[0] = 0;//goal_x;
|
||||||
|
Command_to_Takeoff.Reference_State.position_ref[1] = 0;//goal_y;
|
||||||
|
Command_to_Takeoff.Reference_State.position_ref[2] = 3;
|
||||||
|
Command_to_Takeoff.Reference_State.velocity_ref[0] = 0;
|
||||||
|
Command_to_Takeoff.Reference_State.velocity_ref[1] = 0;
|
||||||
|
Command_to_Takeoff.Reference_State.velocity_ref[2] = 0;
|
||||||
|
Command_to_Takeoff.Reference_State.acceleration_ref[0] = 0;
|
||||||
|
Command_to_Takeoff.Reference_State.acceleration_ref[1] = 0;
|
||||||
|
Command_to_Takeoff.Reference_State.acceleration_ref[2] = 0;
|
||||||
|
Command_to_Takeoff.Reference_State.yaw_ref = 0.0;
|
||||||
|
//Move
|
||||||
|
Command_to_Move.Mode = prometheus_msgs::ControlCommand::Move;
|
||||||
|
Command_to_Move.source = NODE_NAME;
|
||||||
|
Command_to_Move.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
|
||||||
|
Command_to_Move.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
|
||||||
|
Command_to_Move.Reference_State.position_ref[0] = 10;//goal_x;
|
||||||
|
Command_to_Move.Reference_State.position_ref[1] = 0;//goal_y;
|
||||||
|
Command_to_Move.Reference_State.position_ref[2] = 3;
|
||||||
|
Command_to_Move.Reference_State.velocity_ref[0] = 0;
|
||||||
|
Command_to_Move.Reference_State.velocity_ref[1] = 0;
|
||||||
|
Command_to_Move.Reference_State.velocity_ref[2] = 0;
|
||||||
|
Command_to_Move.Reference_State.acceleration_ref[0] = 0;
|
||||||
|
Command_to_Move.Reference_State.acceleration_ref[1] = 0;
|
||||||
|
Command_to_Move.Reference_State.acceleration_ref[2] = 0;
|
||||||
|
Command_to_Move.Reference_State.yaw_ref = 0.0;
|
||||||
|
//Arrive check and return
|
||||||
|
Command_to_Return.Mode = prometheus_msgs::ControlCommand::Move;
|
||||||
|
Command_to_Return.source = NODE_NAME;
|
||||||
|
Command_to_Return.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
|
||||||
|
Command_to_Return.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
|
||||||
|
Command_to_Return.Reference_State.position_ref[0] = 0;
|
||||||
|
Command_to_Return.Reference_State.position_ref[1] = 0;
|
||||||
|
Command_to_Return.Reference_State.position_ref[2] = Command_to_Move.Reference_State.position_ref[2];
|
||||||
|
Command_to_Return.Reference_State.velocity_ref[0] = 0;
|
||||||
|
Command_to_Return.Reference_State.velocity_ref[1] = 0;
|
||||||
|
Command_to_Return.Reference_State.velocity_ref[2] = 0;
|
||||||
|
Command_to_Return.Reference_State.acceleration_ref[0] = 0;
|
||||||
|
Command_to_Return.Reference_State.acceleration_ref[1] = 0;
|
||||||
|
Command_to_Return.Reference_State.acceleration_ref[2] = 0;
|
||||||
|
Command_to_Return.Reference_State.yaw_ref = 0.0;
|
||||||
|
//Land
|
||||||
|
Command_to_Return_Land.Mode = prometheus_msgs::ControlCommand::Land;
|
||||||
|
Command_to_Return_Land.source = NODE_NAME;
|
||||||
|
Command_to_Return_Land.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
|
||||||
|
Command_to_Return_Land.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
|
||||||
|
Command_to_Return_Land.Reference_State.velocity_ref[0] = 0;
|
||||||
|
Command_to_Return_Land.Reference_State.velocity_ref[1] = 0;
|
||||||
|
Command_to_Return_Land.Reference_State.velocity_ref[2] = 0;
|
||||||
|
Command_to_Return_Land.Reference_State.acceleration_ref[0] = 0;
|
||||||
|
Command_to_Return_Land.Reference_State.acceleration_ref[1] = 0;
|
||||||
|
Command_to_Return_Land.Reference_State.acceleration_ref[2] = 0;
|
||||||
|
Command_to_Return_Land.Reference_State.yaw_ref = 0.0;
|
||||||
|
//Hold
|
||||||
|
Command_to_Hold.Mode = prometheus_msgs::ControlCommand::Hold;
|
||||||
|
Command_to_Hold.source = NODE_NAME;
|
||||||
|
Command_to_Hold.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
|
||||||
|
Command_to_Hold.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
|
||||||
|
Command_to_Hold.Reference_State.position_ref[0] = _DroneState.position[0];
|
||||||
|
Command_to_Hold.Reference_State.position_ref[1] = _DroneState.position[1];
|
||||||
|
Command_to_Hold.Reference_State.position_ref[2] = _DroneState.position[2];
|
||||||
|
Command_to_Hold.Reference_State.velocity_ref[0] = 0;
|
||||||
|
Command_to_Hold.Reference_State.velocity_ref[1] = 0;
|
||||||
|
Command_to_Hold.Reference_State.velocity_ref[2] = 0;
|
||||||
|
Command_to_Hold.Reference_State.acceleration_ref[0] = 0;
|
||||||
|
Command_to_Hold.Reference_State.acceleration_ref[1] = 0;
|
||||||
|
Command_to_Hold.Reference_State.acceleration_ref[2] = 0;
|
||||||
|
Command_to_Hold.Reference_State.yaw_ref = 0.0;
|
||||||
|
// 循环发布消息
|
||||||
|
|
||||||
|
while (ros::ok())
|
||||||
|
{
|
||||||
|
clock_t start_time, end_time;
|
||||||
|
ros::AsyncSpinner spinner(1); // 创建一个只有一个线程的 Spinner
|
||||||
|
spinner.start();
|
||||||
|
ros::spinOnce();
|
||||||
|
//int buffer_size = 1024;
|
||||||
|
pthread_t tcp_rec;
|
||||||
|
int tcp_ret = pthread_create(&tcp_rec, NULL, TCP_recv, NULL);
|
||||||
|
// 创建一个新的 prometheus_msgs::ControlCommand 消息
|
||||||
|
//发布消息
|
||||||
|
time_t now = time(NULL);
|
||||||
|
struct tm *local = localtime(&now);
|
||||||
|
ROS_INFO("%d,%d",local->tm_hour,local->tm_min);
|
||||||
|
if(local->tm_hour == hours && local->tm_min == minutes && local->tm_sec == seconds){
|
||||||
|
control_offboard.publish(Command_to_offboard);
|
||||||
|
ROS_INFO("received");
|
||||||
|
sleep(1);
|
||||||
|
control_offboard.publish(Command_to_Takeoff);
|
||||||
|
sleep(20);
|
||||||
|
control_offboard.publish(Command_to_Move);
|
||||||
|
start_time = clock();
|
||||||
|
while(true){
|
||||||
|
end_time = clock();
|
||||||
|
double running_time = (double)(end_time - start_time) / CLOCKS_PER_SEC;
|
||||||
|
if(running_time <= 30){
|
||||||
|
if(hold_flag == 1){
|
||||||
|
control_offboard.publish(Command_to_Hold);
|
||||||
|
hold_flag = 0;
|
||||||
|
arrival_flag = 0;
|
||||||
|
loop_rate.sleep();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//ROS_INFO("check_arrival:%d",arrival_flag);
|
||||||
|
if(arrival_flag == 1){
|
||||||
|
ROS_INFO("start return");
|
||||||
|
sleep(5);
|
||||||
|
control_offboard.publish(Command_to_Return);
|
||||||
|
ROS_INFO("arrival");
|
||||||
|
loop_rate.sleep();
|
||||||
|
loop_rate.sleep();
|
||||||
|
start_time = clock();
|
||||||
|
while(true){
|
||||||
|
end_time = clock();
|
||||||
|
double running_time = (double)(end_time - start_time) / CLOCKS_PER_SEC;
|
||||||
|
if(running_time <= 30){
|
||||||
|
if(hold_flag == 1){
|
||||||
|
control_offboard.publish(Command_to_Hold);
|
||||||
|
/*cJSON *find = cJSON_CreateObject();
|
||||||
|
cJSON_AddStringToObject(find, "latitude", 0);
|
||||||
|
cJSON_AddNumberToObject(find, "longitude", 25);
|
||||||
|
char *json_str = cJSON_Print(find);
|
||||||
|
client.send(json_str);
|
||||||
|
hold_flag = 0;*/
|
||||||
|
loop_rate.sleep();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
control_offboard.publish(Command_to_Return_Land);
|
||||||
|
arrival_flag = 0;
|
||||||
|
}
|
||||||
|
/*if(hold_flag ==1){
|
||||||
|
control_offboard.publish(Command_to_Hold);
|
||||||
|
loop_rate.sleep();
|
||||||
|
hold_flag = 0;
|
||||||
|
}*/
|
||||||
|
// 延时等待,以便控制发布速度
|
||||||
|
loop_rate.sleep();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
Loading…
Reference in new issue