/* receive data from X-Plane via UDP */ bool XPlane::receive_data(void) { uint8_t pkt[10000]; uint8_t *p = &pkt[5]; const uint8_t pkt_len = 36; uint64_t data_mask = 0; const uint64_t one = 1U; const uint64_t required_mask = (one<<Times | one<<LatLonAlt | one<<Speed | one<<PitchRollHeading | one<<LocVelDistTraveled | one<<AngularVelocities | one<<Gload | one << Joystick1 | one << ThrottleCommand | one << Trim | one << PropPitch | one << EngineRPM | one << PropRPM | one << Generator); Location loc {}; Vector3f pos; uint32_t wait_time_ms = 1; uint32_t now = AP_HAL::millis(); // if we are about to get another frame from X-Plane then wait longer if (xplane_frame_time > wait_time_ms && now+1 >= last_data_time_ms + xplane_frame_time) { wait_time_ms = 10; } ssize_t len = socket_in.recv(pkt, sizeof(pkt), wait_time_ms); if (len < pkt_len+5 || memcmp(pkt, "DATA@", 5) != 0) { // not a data packet we understand goto failed; } len -= 5; if (!connected) { // we now know the IP X-Plane is using uint16_t port; socket_in.last_recv_address(xplane_ip, port); socket_out.connect(xplane_ip, xplane_port); connected = true; printf("Connected to %s:%u\n", xplane_ip, (unsigned)xplane_port); } while (len >= pkt_len) { const float *data = (const float *)p; uint8_t code = p[0]; // keep a mask of what codes we have received if (code < 64) { data_mask |= (((uint64_t)1) << code); } switch (code) { case Times: { uint64_t tus = data[3] * 1.0e6f; if (tus + time_base_us <= time_now_us) { uint64_t tdiff = time_now_us - (tus + time_base_us); if (tdiff > 1e6f) { printf("X-Plane time reset %lu\n", (unsigned long)tdiff); } time_base_us = time_now_us - tus; } uint64_t tnew = time_base_us + tus; //uint64_t dt = tnew - time_now_us; //printf("dt %u\n", (unsigned)dt); time_now_us = tnew; break; } case LatLonAlt: { loc.lat = data[1] * 1e7; loc.lng = data[2] * 1e7; loc.alt = data[3] * FEET_TO_METERS * 100.0f; float hagl = data[4] * FEET_TO_METERS; ground_level = loc.alt * 0.01f - hagl; break; } case Speed: airspeed = data[2] * KNOTS_TO_METERS_PER_SECOND; airspeed_pitot = airspeed; break; case AoA: // ignored break; case Trim: if (heli_frame) { // use flaps for collective as no direct collective data input rcin[2] = data[4]; } break; case PitchRollHeading: { float roll, pitch, yaw; pitch = radians(data[1]); roll = radians(data[2]); yaw = radians(data[3]); dcm.from_euler(roll, pitch, yaw); break; } case AtmosphereWeather: // ignored break; case LocVelDistTraveled: pos.y = data[1]; pos.z = -data[2]; pos.x = -data[3]; velocity_ef.y = data[4]; velocity_ef.z = -data[5]; velocity_ef.x = -data[6]; break; case AngularVelocities: gyro.y = data[1]; gyro.x = data[2]; gyro.z = data[3]; break; case Gload: accel_body.z = -data[5] * GRAVITY_MSS; accel_body.x = data[6] * GRAVITY_MSS; accel_body.y = data[7] * GRAVITY_MSS; break; case Joystick1: rcin_chan_count = 4; rcin[0] = (data[2] + 1)*0.5f; rcin[1] = (data[1] + 1)*0.5f; rcin[3] = (data[3] + 1)*0.5f; break; case ThrottleCommand: { if (!heli_frame) { /* getting joystick throttle input is very weird. The * problem is that XPlane sends the ThrottleCommand packet * both for joystick throttle input and for throttle that * we have provided over the link. So we need some way to * detect when we get our own values back. The trick used * is to add throttle_magic to the values we send, then * detect this offset in the data coming back. Very ugly, * but I can't find a better way of allowing joystick * input from XPlane10 */ bool has_magic = ((uint32_t)(data[1] * throttle_magic_scale) % 1000U) == (uint32_t)(throttle_magic * throttle_magic_scale); if (data[1] < 0 || data[1] == throttle_sent || has_magic) { break; } rcin[2] = data[1]; } break; } case PropPitch: { break; } case EngineRPM: rpm1 = data[1]; break; case PropRPM: rpm2 = data[1]; break; case Joystick2: break; case Generator: /* in order to get interlock switch on helis we map the "generator1 on/off" function of XPlane 10 to channel 8. */ rcin_chan_count = 8; rcin[7] = data[1]; break; } len -= pkt_len; p += pkt_len; } if (data_mask != required_mask) { // ask XPlane to change what data it sends select_data(data_mask & ~required_mask, required_mask & ~data_mask); goto failed; } position = pos + position_zero; update_position(); accel_earth = dcm * accel_body; accel_earth.z += GRAVITY_MSS; // the position may slowly deviate due to float accuracy and longitude scaling if (get_distance(loc, location) > 4 || fabsf(loc.alt - location.alt)*0.01 > 2) { printf("X-Plane home reset dist=%f alt=%.1f/%.1f\n", get_distance(loc, location), loc.alt*0.01f, location.alt*0.01f); // reset home location position_zero(-pos.x, -pos.y, -pos.z); home.lat = loc.lat; home.lng = loc.lng; home.alt = loc.alt; position.x = 0; position.y = 0; position.z = 0; update_position(); } update_mag_field_bf(); if (now > last_data_time_ms && now - last_data_time_ms < 100) { xplane_frame_time = now - last_data_time_ms; } last_data_time_ms = AP_HAL::millis(); report.data_count++; report.frame_count++; return true; failed: if (AP_HAL::millis() - last_data_time_ms > 200) { // don't extrapolate beyond 0.2s return false; } // advance time by 1ms Vector3f rot_accel; frame_time_us = 1000; float delta_time = frame_time_us * 1e-6f; time_now_us += frame_time_us; // extrapolate sensors dcm.rotate(gyro * delta_time); dcm.normalize(); // work out acceleration as seen by the accelerometers. It sees the kinematic // acceleration (ie. real movement), plus gravity accel_body = dcm.transposed() * (accel_earth + Vector3f(0,0,-GRAVITY_MSS)); // new velocity and position vectors velocity_ef += accel_earth * delta_time; position += velocity_ef * delta_time; velocity_air_ef = velocity_ef - wind_ef; velocity_air_bf = dcm.transposed() * velocity_air_ef; update_position(); update_mag_field_bf(); report.frame_count++; return false; }
/** execute_db_operator takes as input the db_operator and executes the query. * It should return the result (currently as a char*, although I'm not clear * on what the return type should be, maybe a result struct, and then have * a serialization into a string message). **/ char* execute_db_operator(db_operator* query) { status s; if (query->type == INSERT) { table* tbl1 = query->tables[0]; for(size_t i = 0; i < tbl1->col_count; i++) { s = col_insert(query->columns[i], query->value1[i]); if (s.code != OK) { return s.error_message; } } return "Rows successfully inserted."; } else if (query->type == SELECT) { result* r = malloc(sizeof(struct result)); if (query->columns) { s = select_data(query, &r); } else if (query->result1 && query->result2) { s = vec_scan(query, &r); } else { return "Cannot perform select\n"; } if (s.code != OK) { return s.error_message; } int idx = catalogs[0]->var_count; catalogs[0]->names[idx] = query->name1; catalogs[0]->results[idx] = r; catalogs[0]->var_count++; } else if (query->type == PROJECT) { result* r = malloc(sizeof(struct result)); status s = fetch(*(query->columns), query->result1->payload, query->result1->num_tuples, &r); if (s.code != OK) { return s.error_message; } int idx = catalogs[0]->var_count; catalogs[0]->names[idx] = query->name1; catalogs[0]->results[idx] = r; catalogs[0]->var_count++; } else if (query->type == ADD) { result* r = malloc(sizeof(struct result)); s = add_col((int*)query->result1->payload, (int*)query->result2->payload, query->result1->num_tuples, &r); if (s.code != OK) { return s.error_message; } int idx = catalogs[0]->var_count; catalogs[0]->names[idx] = query->name1; catalogs[0]->results[idx] = r; // for (size_t i = 0; i < r->num_tuples; ++i) // { // printf("%ld\n", ((long*)r->payload)[i]); // } catalogs[0]->var_count++; } else if (query->type == SUB) { result* r = malloc(sizeof(struct result)); s = sub_col((int*)query->result1->payload, (int*)query->result2->payload, query->result1->num_tuples, &r); if (s.code != OK) { return s.error_message; } int idx = catalogs[0]->var_count; catalogs[0]->names[idx] = query->name1; catalogs[0]->results[idx] = r; catalogs[0]->var_count++; } else if (query->type == AGGREGATE) { result* r = malloc(sizeof(struct result)); if (query->agg == MIN) { s = min_col(query->result1, &r); } else if (query->agg == MAX) { s = max_col(query->result1, &r); } else if (query->agg == AVG) { s = avg_col(query->result1, &r); } else if (query->agg == CNT) { s = count_col(query->result1->num_tuples, &r); } else { return "Failed aggregation"; } if (s.code != OK) { return s.error_message; } int idx = catalogs[0]->var_count; catalogs[0]->names[idx] = query->name1; catalogs[0]->results[idx] = r; catalogs[0]->var_count++; } return "Success"; }