/*
update location from position
*/
void Aircraft::update_position(void)
{
location = home;
location_offset(location, position.x, position.y);
location.alt = home.alt - position.z*100.0f;
// we only advance time if it hasn't been advanced already by the
// backend
if (last_time_us == time_now_us) {
time_now_us += frame_time_us;
}
last_time_us = time_now_us;
if (use_time_sync) {
sync_frame_time();
}
#if 0
// logging of raw sitl data
Vector3f accel_ef = dcm * accel_body;
DataFlash_Class::instance()->Log_Write("SITL", "TimeUS,VN,VE,VD,AN,AE,AD,PN,PE,PD", "Qfffffffff",
AP_HAL::micros64(),
velocity_ef.x, velocity_ef.y, velocity_ef.z,
accel_ef.x, accel_ef.y, accel_ef.z,
position.x, position.y, position.z);
#endif
}
/*
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 location from position
*/
void Aircraft::update_position(void)
{
float bearing = degrees(atan2f(position.y, position.x));
float distance = sqrtf(sq(position.x) + sq(position.y));
location = home;
location_update(location, bearing, distance);
location.alt = home.alt - position.z*100.0f;
// we only advance time if it hasn't been advanced already by the
// backend
if (last_time_us == time_now_us) {
time_now_us += frame_time_us;
}
last_time_us = time_now_us;
sync_frame_time();
}
/*
update the JSBSim simulation by one time step
*/
void JSBSim::update(const struct sitl_input &input)
{
while (!initialised) {
if (!create_templates() ||
!start_JSBSim() ||
!open_control_socket() ||
!open_fdm_socket()) {
time_now_us = 1;
return;
}
initialised = true;
}
send_servos(input);
recv_fdm(input);
adjust_frame_time(1000);
sync_frame_time();
drain_control_socket();
}
/*
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();
}
