struct json_iter
json_parse(struct json_pair *p, const struct json_iter* it)
{
struct json_iter next;
next = json_read(&p->name, it);
if (next.err)
return next;
return json_read(&p->value, &next);
}
static muse_cell json_read_array_items( muse_env *env, muse_port_t p, muse_cell h, muse_cell t, int N )
{
int i;
if ( port_eof(p) ) {
return muse_raise_error( env, _csymbol(L"json:end-of-file-in-array"), MUSE_NIL );
} else {
muse_char c = port_getchar(p);
if ( c == ']' ) {
muse_cell v = muse_mk_vector( env, N );
for ( i = 0; i < N; ++i ) {
muse_vector_put( env, v, i, _next(&h) );
}
return v;
} else {
port_ungetchar( c, p );
}
}
if ( h ) {
int sp = _spos();
muse_cell n = _cons( json_read(p), MUSE_NIL );
_sett( t, n );
t = n;
_unwind(sp);
} else {
h = t = _cons( json_read(p), MUSE_NIL );
}
json_skip_whitespace(p);
if ( port_eof(p) ) {
return muse_raise_error( env, _csymbol(L"json:end-of-file-in-array"), h );
} else {
muse_char c = port_getchar(p);
if ( c == ',' ) {
return json_read_array_items( env, p, h, t, N+1 );
} else if ( c == ']' ) {
port_ungetc( c, p );
return json_read_array_items( env, p, h, t, N+1 );
} else {
return muse_raise_error( env, _csymbol(L"json:array-syntax-error"), h );
}
}
}
/**
* @code (read-json [port]) @endcode
*
* Reads and returns a JSON compatible object - which is
* either a number, a string, a vector or a hashtable.
* The values in the vectors and hashtables themselves
* must be JSON compatible objects.
*
* If you have nested containers expressed in JSON, you
* can use the (get ...) function to index deeply into
* the structure. For example,
* @code
* > (define j (read-json))
* {"kind":"prime-numbers","message":[2,3,5,7,11,13,17]}
* {hashtable '((message . {vector 2 3 5 7 11 13 17}) (kind . "prime-numbers"))}
* > (get j 'message 4)
* 11
* @endcode
*
* @note Supports \ref fn_the "the"
*/
muse_cell fn_read_json( muse_env *env, void *context, muse_cell args )
{
muse_port_t p = NULL;
if ( args ) {
p = muse_port( env, _evalnext(&args) );
} else {
p = muse_current_port( env, MUSE_STDIN_PORT, NULL );
}
muse_push_recent_scope( env );
return muse_pop_recent_scope( env, (muse_int)fn_read_json, json_read(p) );
}
int main(int argc, char *argv[]) {
JSON *j;
if(argc < 2)
return EXIT_FAILURE;
j = json_read(argv[1]);
if(!j) {
json_error("Unable to parse %s", argv[1]);
return EXIT_FAILURE;
} else {
json_dump(j);
json_free(j);
return EXIT_SUCCESS;
}
}
static muse_cell json_read_object_items( muse_env *env, muse_port_t p, muse_cell table )
{
json_skip_whitespace(p);
if ( port_eof(p) )
return muse_raise_error( env, _csymbol(L"json:end-of-file-in-object"), MUSE_NIL );
else {
muse_char c = port_getchar(p);
if ( c == '}' ) {
return table;
} else {
int sp = _spos();
muse_cell key, value;
port_ungetchar(c,p);
key = json_read_key(p);
json_skip_whitespace(p);
if ( port_eof(p) ) {
return muse_raise_error( env, _csymbol(L"json:end-of-file-in-object"), MUSE_NIL );
} else {
muse_char c = port_getchar(p);
if ( c == ':' ) {
value = json_read(p);
muse_hashtable_put( env, table, key, value );
_unwind(sp);
{
muse_char c = port_getchar(p);
if ( c == ',' ) {
return json_read_object_items( env, p, table );
} else if ( c == '}' ) {
return table;
} else {
return muse_raise_error( env, _csymbol(L"json:object-syntax-error"), table );
}
}
} else {
return muse_raise_error( env, _csymbol(L"json:object-syntax-error"), table );
}
}
}
}
}
int read_json(int argc, char **argv, char *fname, const char *layername, int maxzoom, int minzoom, sqlite3 *outdb, struct pool *exclude, struct pool *include, int exclude_all, double droprate, int buffer, const char *tmpdir, double gamma, char *prevent) {
int ret = EXIT_SUCCESS;
char metaname[strlen(tmpdir) + strlen("/meta.XXXXXXXX") + 1];
char geomname[strlen(tmpdir) + strlen("/geom.XXXXXXXX") + 1];
char indexname[strlen(tmpdir) + strlen("/index.XXXXXXXX") + 1];
sprintf(metaname, "%s%s", tmpdir, "/meta.XXXXXXXX");
sprintf(geomname, "%s%s", tmpdir, "/geom.XXXXXXXX");
sprintf(indexname, "%s%s", tmpdir, "/index.XXXXXXXX");
int metafd = mkstemp(metaname);
if (metafd < 0) {
perror(metaname);
exit(EXIT_FAILURE);
}
int geomfd = mkstemp(geomname);
if (geomfd < 0) {
perror(geomname);
exit(EXIT_FAILURE);
}
int indexfd = mkstemp(indexname);
if (indexfd < 0) {
perror(indexname);
exit(EXIT_FAILURE);
}
FILE *metafile = fopen(metaname, "wb");
if (metafile == NULL) {
perror(metaname);
exit(EXIT_FAILURE);
}
FILE *geomfile = fopen(geomname, "wb");
if (geomfile == NULL) {
perror(geomname);
exit(EXIT_FAILURE);
}
FILE *indexfile = fopen(indexname, "wb");
if (indexfile == NULL) {
perror(indexname);
exit(EXIT_FAILURE);
}
long long metapos = 0;
long long geompos = 0;
long long indexpos = 0;
unlink(metaname);
unlink(geomname);
unlink(indexname);
unsigned file_bbox[] = { UINT_MAX, UINT_MAX, 0, 0 };
unsigned midx = 0, midy = 0;
long long seq = 0;
int nlayers = argc;
if (nlayers == 0) {
nlayers = 1;
}
int n;
for (n = 0; n < nlayers; n++) {
json_pull *jp;
const char *reading;
FILE *fp;
long long found_hashes = 0;
long long found_features = 0;
if (n >= argc) {
reading = "standard input";
fp = stdin;
} else {
reading = argv[n];
fp = fopen(argv[n], "r");
if (fp == NULL) {
perror(argv[n]);
continue;
}
}
jp = json_begin_file(fp);
while (1) {
json_object *j = json_read(jp);
if (j == NULL) {
if (jp->error != NULL) {
fprintf(stderr, "%s:%d: %s\n", reading, jp->line, jp->error);
}
json_free(jp->root);
break;
}
if (j->type == JSON_HASH) {
found_hashes++;
if (found_hashes == 50 && found_features == 0) {
fprintf(stderr, "%s:%d: Not finding any GeoJSON features in input. Is your file just bare geometries?\n", reading, jp->line);
break;
}
}
json_object *type = json_hash_get(j, "type");
if (type == NULL || type->type != JSON_STRING || strcmp(type->string, "Feature") != 0) {
continue;
}
found_features++;
json_object *geometry = json_hash_get(j, "geometry");
if (geometry == NULL) {
fprintf(stderr, "%s:%d: feature with no geometry\n", reading, jp->line);
json_free(j);
continue;
}
json_object *geometry_type = json_hash_get(geometry, "type");
if (geometry_type == NULL) {
static int warned = 0;
if (!warned) {
fprintf(stderr, "%s:%d: null geometry (additional not reported)\n", reading, jp->line);
warned = 1;
}
json_free(j);
continue;
}
if (geometry_type->type != JSON_STRING) {
fprintf(stderr, "%s:%d: geometry without type\n", reading, jp->line);
json_free(j);
continue;
}
json_object *properties = json_hash_get(j, "properties");
if (properties == NULL || (properties->type != JSON_HASH && properties->type != JSON_NULL)) {
fprintf(stderr, "%s:%d: feature without properties hash\n", reading, jp->line);
json_free(j);
continue;
}
json_object *coordinates = json_hash_get(geometry, "coordinates");
if (coordinates == NULL || coordinates->type != JSON_ARRAY) {
fprintf(stderr, "%s:%d: feature without coordinates array\n", reading, jp->line);
json_free(j);
continue;
}
int t;
for (t = 0; t < GEOM_TYPES; t++) {
if (strcmp(geometry_type->string, geometry_names[t]) == 0) {
break;
}
}
if (t >= GEOM_TYPES) {
fprintf(stderr, "%s:%d: Can't handle geometry type %s\n", reading, jp->line, geometry_type->string);
json_free(j);
continue;
}
{
unsigned bbox[] = { UINT_MAX, UINT_MAX, 0, 0 };
int nprop = 0;
if (properties->type == JSON_HASH) {
nprop = properties->length;
}
long long metastart = metapos;
char *metakey[nprop];
char *metaval[nprop];
int metatype[nprop];
int m = 0;
int i;
for (i = 0; i < nprop; i++) {
if (properties->keys[i]->type == JSON_STRING) {
if (exclude_all) {
if (!is_pooled(include, properties->keys[i]->string, VT_STRING)) {
continue;
}
} else if (is_pooled(exclude, properties->keys[i]->string, VT_STRING)) {
continue;
}
metakey[m] = properties->keys[i]->string;
if (properties->values[i] != NULL && properties->values[i]->type == JSON_STRING) {
metatype[m] = VT_STRING;
metaval[m] = properties->values[i]->string;
m++;
} else if (properties->values[i] != NULL && properties->values[i]->type == JSON_NUMBER) {
metatype[m] = VT_NUMBER;
metaval[m] = properties->values[i]->string;
m++;
} else if (properties->values[i] != NULL && (properties->values[i]->type == JSON_TRUE || properties->values[i]->type == JSON_FALSE)) {
metatype[m] = VT_BOOLEAN;
metaval[m] = properties->values[i]->type == JSON_TRUE ? "true" : "false";
m++;
} else if (properties->values[i] != NULL && (properties->values[i]->type == JSON_NULL)) {
;
} else {
fprintf(stderr, "%s:%d: Unsupported property type for %s\n", reading, jp->line, properties->keys[i]->string);
json_free(j);
continue;
}
}
}
serialize_int(metafile, m, &metapos, fname);
for (i = 0; i < m; i++) {
serialize_int(metafile, metatype[i], &metapos, fname);
serialize_string(metafile, metakey[i], &metapos, fname);
serialize_string(metafile, metaval[i], &metapos, fname);
}
long long geomstart = geompos;
serialize_byte(geomfile, mb_geometry[t], &geompos, fname);
serialize_byte(geomfile, n, &geompos, fname);
serialize_long_long(geomfile, metastart, &geompos, fname);
parse_geometry(t, coordinates, bbox, &geompos, geomfile, VT_MOVETO, fname, jp);
serialize_byte(geomfile, VT_END, &geompos, fname);
/*
* Note that minzoom for lines is the dimension
* of the geometry in world coordinates, but
* for points is the lowest zoom level (in tiles,
* not in pixels) at which it should be drawn.
*
* So a line that is too small for, say, z8
* will have minzoom of 18 (if tile detail is 10),
* not 8.
*/
int minzoom = 0;
if (mb_geometry[t] == VT_LINE) {
for (minzoom = 0; minzoom < 31; minzoom++) {
unsigned mask = 1 << (32 - (minzoom + 1));
if (((bbox[0] & mask) != (bbox[2] & mask)) ||
((bbox[1] & mask) != (bbox[3] & mask))) {
break;
}
}
} else if (mb_geometry[t] == VT_POINT) {
double r = ((double) rand()) / RAND_MAX;
if (r == 0) {
r = .00000001;
}
minzoom = maxzoom - floor(log(r) / - log(droprate));
}
serialize_byte(geomfile, minzoom, &geompos, fname);
struct index index;
index.start = geomstart;
index.end = geompos;
index.index = encode(bbox[0] / 2 + bbox[2] / 2, bbox[1] / 2 + bbox[3] / 2);
fwrite_check(&index, sizeof(struct index), 1, indexfile, fname);
indexpos += sizeof(struct index);
for (i = 0; i < 2; i++) {
if (bbox[i] < file_bbox[i]) {
file_bbox[i] = bbox[i];
}
}
for (i = 2; i < 4; i++) {
if (bbox[i] > file_bbox[i]) {
file_bbox[i] = bbox[i];
}
}
if (seq % 10000 == 0) {
fprintf(stderr, "Read %.2f million features\r", seq / 1000000.0);
}
seq++;
}
json_free(j);
/* XXX check for any non-features in the outer object */
}
json_end(jp);
fclose(fp);
}
fclose(metafile);
fclose(geomfile);
fclose(indexfile);
struct stat geomst;
struct stat metast;
if (fstat(geomfd, &geomst) != 0) {
perror("stat geom\n");
exit(EXIT_FAILURE);
}
if (fstat(metafd, &metast) != 0) {
perror("stat meta\n");
exit(EXIT_FAILURE);
}
if (geomst.st_size == 0 || metast.st_size == 0) {
fprintf(stderr, "did not read any valid geometries\n");
exit(EXIT_FAILURE);
}
char *meta = (char *) mmap(NULL, metast.st_size, PROT_READ, MAP_PRIVATE, metafd, 0);
if (meta == MAP_FAILED) {
perror("mmap meta");
exit(EXIT_FAILURE);
}
struct pool file_keys1[nlayers];
struct pool *file_keys[nlayers];
int i;
for (i = 0; i < nlayers; i++) {
pool_init(&file_keys1[i], 0);
file_keys[i] = &file_keys1[i];
}
char *layernames[nlayers];
for (i = 0; i < nlayers; i++) {
if (argc <= 1 && layername != NULL) {
layernames[i] = strdup(layername);
} else {
char *src = argv[i];
if (argc < 1) {
src = fname;
}
char *trunc = layernames[i] = malloc(strlen(src) + 1);
const char *ocp, *use = src;
for (ocp = src; *ocp; ocp++) {
if (*ocp == '/' && ocp[1] != '\0') {
use = ocp + 1;
}
}
strcpy(trunc, use);
char *cp = strstr(trunc, ".json");
if (cp != NULL) {
*cp = '\0';
}
cp = strstr(trunc, ".mbtiles");
if (cp != NULL) {
*cp = '\0';
}
layername = trunc;
char *out = trunc;
for (cp = trunc; *cp; cp++) {
if (isalpha(*cp) || isdigit(*cp) || *cp == '_') {
*out++ = *cp;
}
}
*out = '\0';
printf("using layer %d name %s\n", i, trunc);
}
}
/* Sort the index by geometry */
{
int bytes = sizeof(struct index);
fprintf(stderr, "Sorting %lld features\n", (long long) indexpos / bytes);
int page = sysconf(_SC_PAGESIZE);
long long unit = (50 * 1024 * 1024 / bytes) * bytes;
while (unit % page != 0) {
unit += bytes;
}
int nmerges = (indexpos + unit - 1) / unit;
struct merge merges[nmerges];
long long start;
for (start = 0; start < indexpos; start += unit) {
long long end = start + unit;
if (end > indexpos) {
end = indexpos;
}
if (nmerges != 1) {
fprintf(stderr, "Sorting part %lld of %d\r", start / unit + 1, nmerges);
}
merges[start / unit].start = start;
merges[start / unit].end = end;
merges[start / unit].next = NULL;
void *map = mmap(NULL, end - start, PROT_READ | PROT_WRITE, MAP_PRIVATE, indexfd, start);
if (map == MAP_FAILED) {
perror("mmap");
exit(EXIT_FAILURE);
}
qsort(map, (end - start) / bytes, bytes, indexcmp);
// Sorting and then copying avoids the need to
// write out intermediate stages of the sort.
void *map2 = mmap(NULL, end - start, PROT_READ | PROT_WRITE, MAP_SHARED, indexfd, start);
if (map2 == MAP_FAILED) {
perror("mmap (write)");
exit(EXIT_FAILURE);
}
memcpy(map2, map, end - start);
munmap(map, end - start);
munmap(map2, end - start);
}
if (nmerges != 1) {
fprintf(stderr, "\n");
}
void *map = mmap(NULL, indexpos, PROT_READ, MAP_PRIVATE, indexfd, 0);
if (map == MAP_FAILED) {
perror("mmap");
exit(EXIT_FAILURE);
}
FILE *f = fopen(indexname, "w");
if (f == NULL) {
perror(indexname);
exit(EXIT_FAILURE);
}
merge(merges, nmerges, (unsigned char *) map, f, bytes, indexpos / bytes);
munmap(map, indexpos);
fclose(f);
close(indexfd);
}
/* Copy geometries to a new file in index order */
indexfd = open(indexname, O_RDONLY);
if (indexfd < 0) {
perror("reopen sorted index");
exit(EXIT_FAILURE);
}
struct index *index_map = mmap(NULL, indexpos, PROT_READ, MAP_PRIVATE, indexfd, 0);
if (index_map == MAP_FAILED) {
perror("mmap index");
exit(EXIT_FAILURE);
}
unlink(indexname);
char *geom_map = mmap(NULL, geomst.st_size, PROT_READ, MAP_PRIVATE, geomfd, 0);
if (geom_map == MAP_FAILED) {
perror("mmap unsorted geometry");
exit(EXIT_FAILURE);
}
if (close(geomfd) != 0) {
perror("close unsorted geometry");
}
sprintf(geomname, "%s%s", tmpdir, "/geom.XXXXXXXX");
geomfd = mkstemp(geomname);
if (geomfd < 0) {
perror(geomname);
exit(EXIT_FAILURE);
}
geomfile = fopen(geomname, "wb");
if (geomfile == NULL) {
perror(geomname);
exit(EXIT_FAILURE);
}
{
geompos = 0;
/* initial tile is 0/0/0 */
serialize_int(geomfile, 0, &geompos, fname);
serialize_uint(geomfile, 0, &geompos, fname);
serialize_uint(geomfile, 0, &geompos, fname);
long long i;
long long sum = 0;
long long progress = 0;
for (i = 0; i < indexpos / sizeof(struct index); i++) {
fwrite_check(geom_map + index_map[i].start, sizeof(char), index_map[i].end - index_map[i].start, geomfile, fname);
sum += index_map[i].end - index_map[i].start;
long long p = 1000 * i / (indexpos / sizeof(struct index));
if (p != progress) {
fprintf(stderr, "Reordering geometry: %3.1f%%\r", p / 10.0);
progress = p;
}
}
/* end of tile */
serialize_byte(geomfile, -2, &geompos, fname);
fclose(geomfile);
}
if (munmap(index_map, indexpos) != 0) {
perror("unmap sorted index");
}
if (munmap(geom_map, geomst.st_size) != 0) {
perror("unmap unsorted geometry");
}
if (close(indexfd) != 0) {
perror("close sorted index");
}
/* Traverse and split the geometries for each zoom level */
geomfd = open(geomname, O_RDONLY);
if (geomfd < 0) {
perror("reopen sorted geometry");
exit(EXIT_FAILURE);
}
unlink(geomname);
if (fstat(geomfd, &geomst) != 0) {
perror("stat sorted geom\n");
exit(EXIT_FAILURE);
}
int fd[4];
off_t size[4];
fd[0] = geomfd;
size[0] = geomst.st_size;
int j;
for (j = 1; j < 4; j++) {
fd[j] = -1;
size[j] = 0;
}
fprintf(stderr, "%lld features, %lld bytes of geometry, %lld bytes of metadata\n", seq, (long long) geomst.st_size, (long long) metast.st_size);
int written = traverse_zooms(fd, size, meta, file_bbox, file_keys, &midx, &midy, layernames, maxzoom, minzoom, outdb, droprate, buffer, fname, tmpdir, gamma, nlayers, prevent);
if (maxzoom != written) {
fprintf(stderr, "\n\n\n*** NOTE TILES ONLY COMPLETE THROUGH ZOOM %d ***\n\n\n", written);
maxzoom = written;
ret = EXIT_FAILURE;
}
if (munmap(meta, metast.st_size) != 0) {
perror("munmap meta");
}
if (close(metafd) < 0) {
perror("close meta");
}
double minlat = 0, minlon = 0, maxlat = 0, maxlon = 0, midlat = 0, midlon = 0;
tile2latlon(midx, midy, maxzoom, &maxlat, &minlon);
tile2latlon(midx + 1, midy + 1, maxzoom, &minlat, &maxlon);
midlat = (maxlat + minlat) / 2;
midlon = (maxlon + minlon) / 2;
tile2latlon(file_bbox[0], file_bbox[1], 32, &maxlat, &minlon);
tile2latlon(file_bbox[2], file_bbox[3], 32, &minlat, &maxlon);
if (midlat < minlat) {
midlat = minlat;
}
if (midlat > maxlat) {
midlat = maxlat;
}
if (midlon < minlon) {
midlon = minlon;
}
if (midlon > maxlon) {
midlon = maxlon;
}
mbtiles_write_metadata(outdb, fname, layernames, minzoom, maxzoom, minlat, minlon, maxlat, maxlon, midlat, midlon, file_keys, nlayers); // XXX layers
for (i = 0; i < nlayers; i++) {
pool_free_strings(&file_keys1[i]);
free(layernames[i]);
}
return ret;
}
/*
* main()
*
* @function DronePi Application main
*
*/
int main(int argc, char* argv[])
{
//yaw pid
int count;
char ff;
int cnt=0;
int ycnt=0;
//network about
int tcp,udp;
int flag,str_len,t=0;
double x = 0,y=0,z=0;
double pp,pi,pd;
char pidbuf[100],tcp_flag,recv_flag;
json_t *data;
json_error_t error;
// Yaw PID 상수
double yp,yi,yd;
// Roll, Pitch PID 상수
double kp, ki, kd;
// 모터 출력량
int throttle = 0;
// 상대좌표 Yaw를 사용하기위한 보정 값
float adjustYaw=0.f;
// Quardcopter 로 모터 4개 선언
Motor motor[4];
// imu 장치 MPU6050 선언
IMU imu;
for(int i = 0 ; i < 4 ; i++)
{
motor[i].init();
}
// Setting Pin to Raspberry pi gpio
motor[0].setPin(22);
motor[1].setPin(23);
motor[2].setPin(24);
motor[3].setPin(25);
// ESC Calibration
motor[0].calibration();
motor[1].calibration();
motor[2].calibration();
motor[3].calibration();
if(imu.init())
{
printf("Plese check the MPU6050 device connection!!\n");
return 0; //센서 연결 실패
}
// PID 연산을 위한 각 축의 PID 연산자 객체 선언
PID rollPid, pitchPid, yawPid;
rollPid.init();
pitchPid.init();
yawPid.init();
kp = 2.0; // 1.2
ki = 0.1; // 0.00084
kd = 0.85; // 0.6134
rollPid.setTuning(2.0, 0.1, 0.78);
pitchPid.setTuning(2.0, 0.1, 0.78);
yawPid.setTuning(10.0, 0.f, 10.0);
float roll, pitch, yaw, preYaw, rYaw;
int pidRoll, pidPitch, pidYaw;
// 자이로센서 init 및 calibration
imu.calibration();
int Motor1_speed, Motor2_speed, Motor3_speed, Motor4_speed;
printf("Connect Application!!\n");
// Network init
if(network_init(&tcp,&udp)==-1)
{
printf("socket create error\n");
return 0;
}
printf("network init complete\n");
// 급격한 각도변화 발생시 비상제어. 착륙 실행 flag
int Emergency = 0;
// 상대 Yaw 좌표
preYaw = 0;
/*
* Drone Control routine
*
* 1. 조작 데이터 수신
* 2. 현재 각도 측정
* 3. PID 연산
* 4. 모터 속도 업데이트
*
*/
while(1)
{
// Get json data from android app
flag=json_read(udp,&x,&y,&z,&t);
if(flag==1)
{
throttle = t;
}
// 2. 현재 각도 측정
imu.getIMUData(&roll, &pitch, &yaw);
// 센서 오류 및 오동작으로 인한 동작 방지
// 특정 각도 이상으로 기울어지면 종료할 수 있도록 함
if( pitch < -50 || pitch > 50 || roll < -50 || roll > 50)
{
Emergency = 1;
}
// 상대 Yaw 연산
if(preYaw>100&&yaw<-100)
rYaw=360+yaw-preYaw;
else if(preYaw<-100&&yaw>100)
rYaw=360-yaw+preYaw;
else
rYaw= yaw - preYaw;
if(ycnt==33)
{
printf("rYaw: %.2lf preYaw: %.2lf yaw: %.2lf\n",rYaw,preYaw,yaw);
ycnt=0;
}
ycnt++;
preYaw = yaw;
// 각 축의 PID 연산
pidRoll = rollPid.calcPID(x, roll);
pidPitch = pitchPid.calcPID(-y, pitch);
pidYaw = yawPid.calcPID(0, rYaw-z);
// Quardcopter type X 의 모터 속도 연산
Motor1_speed = throttle + pidRoll + pidPitch + pidYaw;
Motor2_speed = throttle - pidRoll + pidPitch - pidYaw;
Motor3_speed = throttle - pidRoll - pidPitch + pidYaw;
Motor4_speed = throttle + pidRoll - pidPitch - pidYaw;
if(cnt==33)
{
printf("X: %.2lf Y: %.2lf Z: %.2lf T:%d \n",x,y,z,t);
printf("ROLL : %3.6f, PITCH %3.7f, YAW %3.7f\n", roll, pitch, yaw);
printf("pidRoll : %d, pidPitch : %d pidYaw: %d\n", pidRoll,pidPitch,pidYaw);
printf("motor speed [1]:%d [2]:%d [3]:%d [4]:%d\n\n\n", Motor1_speed, Motor2_speed, Motor3_speed, Motor4_speed);
cnt=0;
}
cnt++;
if(Emergency)
{
// 긴급상황. 모터속도 최소화
Motor1_speed = 700;
Motor2_speed = 700;
Motor3_speed = 700;
Motor4_speed = 700;
}
if(Motor1_speed < 700) Motor1_speed = 700;
if(Motor2_speed < 700) Motor2_speed = 700;
if(Motor3_speed < 700) Motor3_speed = 700;
if(Motor4_speed < 700) Motor4_speed = 700;
// 모터 속도 업데이트
motor[0].setSpeed(Motor1_speed);
motor[1].setSpeed(Motor2_speed);
motor[2].setSpeed(Motor3_speed);
motor[3].setSpeed(Motor4_speed);
tcp_flag=tcp_read(tcp);
if(tcp_flag==3)
{
motor[0].setSpeed(700);
motor[1].setSpeed(700);
motor[2].setSpeed(700);
motor[3].setSpeed(700);
memset(pidbuf,0,sizeof(pidbuf));
printf("pid reset flag\n");
str_len=recv(tcp,pidbuf,sizeof(pidbuf),MSG_DONTROUTE);
pidbuf[str_len]=0;
printf("pid reset data :%s\n",pidbuf);
data=json_loads(pidbuf,JSON_DECODE_ANY,&error);
if((json_unpack(data,"{s:f,s:f,s:f,s:f,s:f,s:f}","P_P",&pp,"P_I",&pi,"P_D",&pd,"Y_P",&yp,"Y_I",&yi,"Y_D",&yd))!=0)
{
printf("pid reset error\n");
recv_flag=0;
send(tcp,&recv_flag,1,MSG_DONTROUTE);
return 0;
}
recv_flag=1;
send(tcp,&recv_flag,1,MSG_DONTROUTE);
rollPid.initKpid(pp, pi, pd);
pitchPid.initKpid(pp, pi, pd);
yawPid.initKpid(yp, yi, yd);
Emergency=0;
printf("pid reset complete\n");
}
else if(tcp_flag==5)
{
motor[0].setSpeed(700);
motor[1].setSpeed(700);
motor[2].setSpeed(700);
motor[3].setSpeed(700);
printf("drone exit\n");
break;
}
else if(tcp_flag==0)
{
motor[0].setSpeed(700);
motor[1].setSpeed(700);
motor[2].setSpeed(700);
motor[3].setSpeed(700);
printf("Controller connection less\n");
break;
}
}//end while(1)
sleep(3);
}//end int main()
void parse_json(json_feature_action *jfa, json_pull *jp) {
long long found_hashes = 0;
long long found_features = 0;
long long found_geometries = 0;
while (1) {
json_object *j = json_read(jp);
if (j == NULL) {
if (jp->error != NULL) {
fprintf(stderr, "%s:%d: %s\n", jfa->fname.c_str(), jp->line, jp->error);
if (jp->root != NULL) {
json_context(jp->root);
}
}
json_free(jp->root);
break;
}
if (j->type == JSON_HASH) {
found_hashes++;
if (found_hashes == 50 && found_features == 0 && found_geometries == 0) {
fprintf(stderr, "%s:%d: Warning: not finding any GeoJSON features or geometries in input yet after 50 objects.\n", jfa->fname.c_str(), jp->line);
}
}
json_object *type = json_hash_get(j, "type");
if (type == NULL || type->type != JSON_STRING) {
continue;
}
if (found_features == 0) {
int i;
int is_geometry = 0;
for (i = 0; i < GEOM_TYPES; i++) {
if (strcmp(type->string, geometry_names[i]) == 0) {
is_geometry = 1;
break;
}
}
if (is_geometry) {
if (j->parent != NULL) {
if (j->parent->type == JSON_ARRAY && j->parent->parent != NULL) {
if (j->parent->parent->type == JSON_HASH) {
json_object *geometries = json_hash_get(j->parent->parent, "geometries");
if (geometries != NULL) {
// Parent of Parent must be a GeometryCollection
is_geometry = 0;
}
}
} else if (j->parent->type == JSON_HASH) {
json_object *geometry = json_hash_get(j->parent, "geometry");
if (geometry != NULL) {
// Parent must be a Feature
is_geometry = 0;
}
}
}
}
if (is_geometry) {
json_object *jo = j;
while (jo != NULL) {
if (jo->parent != NULL && jo->parent->type == JSON_HASH) {
if (json_hash_get(jo->parent, "properties") == jo) {
// Ancestor is the value corresponding to a properties key
is_geometry = 0;
break;
}
}
jo = jo->parent;
}
}
if (is_geometry) {
if (found_features != 0 && found_geometries == 0) {
fprintf(stderr, "%s:%d: Warning: found a mixture of features and bare geometries\n", jfa->fname.c_str(), jp->line);
}
found_geometries++;
jfa->add_feature(j, false, NULL, NULL, NULL, j);
json_free(j);
continue;
}
}
if (strcmp(type->string, "Feature") != 0) {
if (strcmp(type->string, "FeatureCollection") == 0) {
jfa->check_crs(j);
json_free(j);
}
continue;
}
if (found_features == 0 && found_geometries != 0) {
fprintf(stderr, "%s:%d: Warning: found a mixture of features and bare geometries\n", jfa->fname.c_str(), jp->line);
}
found_features++;
json_object *geometry = json_hash_get(j, "geometry");
if (geometry == NULL) {
fprintf(stderr, "%s:%d: feature with no geometry\n", jfa->fname.c_str(), jp->line);
json_context(j);
json_free(j);
continue;
}
json_object *properties = json_hash_get(j, "properties");
if (properties == NULL || (properties->type != JSON_HASH && properties->type != JSON_NULL)) {
fprintf(stderr, "%s:%d: feature without properties hash\n", jfa->fname.c_str(), jp->line);
json_context(j);
json_free(j);
continue;
}
bool is_feature = true;
{
json_object *jo = j;
while (jo != NULL) {
if (jo->parent != NULL && jo->parent->type == JSON_HASH) {
if (json_hash_get(jo->parent, "properties") == jo) {
// Ancestor is the value corresponding to a properties key
is_feature = false;
break;
}
}
jo = jo->parent;
}
}
if (!is_feature) {
continue;
}
json_object *tippecanoe = json_hash_get(j, "tippecanoe");
json_object *id = json_hash_get(j, "id");
json_object *geometries = json_hash_get(geometry, "geometries");
if (geometries != NULL && geometries->type == JSON_ARRAY) {
jfa->add_feature(geometries, true, properties, id, tippecanoe, j);
} else {
jfa->add_feature(geometry, false, properties, id, tippecanoe, j);
}
json_free(j);
/* XXX check for any non-features in the outer object */
}
}
