/*
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";
}
