/*
receive an update from the FDM
This is a blocking function
*/
void JSBSim::recv_fdm(const struct sitl_input &input)
{
FGNetFDM fdm;
check_stdout();
while (sock_fgfdm.recv(&fdm, sizeof(fdm), 100) != sizeof(fdm)) {
send_servos(input);
check_stdout();
}
fdm.ByteSwap();
accel_body = Vector3f(fdm.A_X_pilot, fdm.A_Y_pilot, fdm.A_Z_pilot) * FEET_TO_METERS;
double p, q, r;
SITL::convert_body_frame(degrees(fdm.phi), degrees(fdm.theta),
degrees(fdm.phidot), degrees(fdm.thetadot), degrees(fdm.psidot),
&p, &q, &r);
gyro = Vector3f(p, q, r);
velocity_ef = Vector3f(fdm.v_north, fdm.v_east, fdm.v_down) * FEET_TO_METERS;
location.lat = degrees(fdm.latitude) * 1.0e7;
location.lng = degrees(fdm.longitude) * 1.0e7;
location.alt = fdm.agl*100 + home.alt;
dcm.from_euler(fdm.phi, fdm.theta, fdm.psi);
airspeed = fdm.vcas * FEET_TO_METERS;
airspeed_pitot = airspeed;
// update magnetic field
update_mag_field_bf();
rpm1 = fdm.rpm[0];
rpm2 = fdm.rpm[1];
// assume 1kHz for now
time_now_us += 1000;
}
/*
update the Gazebo simulation by one time step
*/
void Gazebo::update(const struct sitl_input &input)
{
send_servos(input);
recv_fdm(input);
update_position();
// update magnetic field
update_mag_field_bf();
}
/*
update the last_letter simulation by one time step
*/
void last_letter::update(const struct sitl_input &input)
{
send_servos(input);
recv_fdm(input);
sync_frame_time();
update_position();
// update magnetic field
update_mag_field_bf();
}
/*
update the helicopter simulation by one time step
*/
void Helicopter::update(const struct sitl_input &input)
{
// get wind vector setup
update_wind(input);
float rsc = constrain_float((input.servos[7]-1000) / 1000.0f, 0, 1);
// ignition only for gas helis
bool ignition_enabled = gas_heli?(input.servos[5] > 1500):true;
float thrust = 0;
float roll_rate = 0;
float pitch_rate = 0;
float yaw_rate = 0;
float torque_effect_accel = 0;
float lateral_x_thrust = 0;
float lateral_y_thrust = 0;
float swash1 = (input.servos[0]-1000) / 1000.0f;
float swash2 = (input.servos[1]-1000) / 1000.0f;
float swash3 = (input.servos[2]-1000) / 1000.0f;
if (!ignition_enabled) {
rsc = 0;
}
float rsc_scale = rsc/rsc_setpoint;
switch (frame_type) {
case HELI_FRAME_CONVENTIONAL: {
// simulate a traditional helicopter
float tail_rotor = (input.servos[3]-1000) / 1000.0f;
thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f;
torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel;
roll_rate = swash1 - swash2;
pitch_rate = (swash1+swash2) / 2.0f - swash3;
yaw_rate = tail_rotor - 0.5f;
lateral_y_thrust = yaw_rate * rsc_scale * tail_thrust_scale;
break;
}
case HELI_FRAME_DUAL: {
// simulate a tandem helicopter
float swash4 = (input.servos[3]-1000) / 1000.0f;
float swash5 = (input.servos[4]-1000) / 1000.0f;
float swash6 = (input.servos[5]-1000) / 1000.0f;
thrust = (rsc / rsc_setpoint) * (swash1+swash2+swash3+swash4+swash5+swash6) / 6.0f;
torque_effect_accel = (rsc_scale + rsc / rsc_setpoint) * rotor_rot_accel * ((swash1+swash2+swash3) - (swash4+swash5+swash6));
roll_rate = (swash1-swash2) + (swash4-swash5);
pitch_rate = (swash1+swash2+swash3) - (swash4+swash5+swash6);
yaw_rate = (swash1-swash2) + (swash5-swash4);
break;
}
case HELI_FRAME_COMPOUND: {
// simulate a compound helicopter
float right_rotor = (input.servos[3]-1000) / 1000.0f;
float left_rotor = (input.servos[4]-1000) / 1000.0f;
thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f;
torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel;
roll_rate = swash1 - swash2;
pitch_rate = (swash1+swash2) / 2.0f - swash3;
yaw_rate = right_rotor - left_rotor;
lateral_x_thrust = (left_rotor+right_rotor-1) * rsc_scale * tail_thrust_scale;
break;
}
}
roll_rate *= rsc_scale;
pitch_rate *= rsc_scale;
yaw_rate *= rsc_scale;
// rotational acceleration, in rad/s/s, in body frame
Vector3f rot_accel;
rot_accel.x = roll_rate * roll_rate_max;
rot_accel.y = pitch_rate * pitch_rate_max;
rot_accel.z = yaw_rate * yaw_rate_max;
// rotational air resistance
rot_accel.x -= gyro.x * radians(5000.0) / terminal_rotation_rate;
rot_accel.y -= gyro.y * radians(5000.0) / terminal_rotation_rate;
rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
// torque effect on tail
rot_accel.z += torque_effect_accel;
// air resistance
Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
// simulate rotor speed
rpm1 = thrust * 1300;
// scale thrust to newtons
thrust *= thrust_scale;
accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass);
accel_body += dcm.transposed() * air_resistance;
update_dynamics(rot_accel);
// update lat/lon/altitude
update_position();
// update magnetic field
update_mag_field_bf();
}
/*
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;
}
