ardupilot/libraries/SITL/SIM_last_letter.cpp

138 lines
3.5 KiB
C++
Raw Normal View History

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
simulator connector for ardupilot version of last_letter
*/
#include "SIM_last_letter.h"
#include <fcntl.h>
#include <stdio.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <AP_HAL/AP_HAL.h>
extern const AP_HAL::HAL& hal;
2015-10-22 10:04:42 -03:00
namespace SITL {
last_letter::last_letter(const char *home_str, const char *_frame_str) :
Aircraft(home_str, _frame_str),
last_timestamp_us(0),
sock(true)
{
// try to bind to a specific port so that if we restart ArduPilot
// last_letter keeps sending us packets. Not strictly necessary but
// useful for debugging
sock.bind("127.0.0.1", fdm_port+1);
sock.reuseaddress();
sock.set_blocking(false);
start_last_letter();
}
/*
start last_letter child
*/
void last_letter::start_last_letter(void)
{
pid_t child_pid = fork();
if (child_pid == 0) {
// in child
close(0);
open("/dev/null", O_RDONLY|O_CLOEXEC);
for (uint8_t i=3; i<100; i++) {
close(i);
}
int ret = execlp("roslaunch",
"roslaunch",
"last_letter",
"gazebo.launch",
"ArduPlane:=true",
nullptr);
if (ret != 0) {
perror("roslaunch");
}
exit(1);
}
}
/*
send servos
*/
void last_letter::send_servos(const struct sitl_input &input)
{
servo_packet pkt;
memcpy(pkt.servos, input.servos, sizeof(pkt.servos));
sock.sendto(&pkt, sizeof(pkt), "127.0.0.1", fdm_port);
}
/*
receive an update from the FDM
This is a blocking function
*/
void last_letter::recv_fdm(const struct sitl_input &input)
{
fdm_packet pkt;
/*
we re-send the servo packet every 0.1 seconds until we get a
reply. This allows us to cope with some packet loss to the FDM
*/
while (sock.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
send_servos(input);
}
accel_body = Vector3f(pkt.xAccel, pkt.yAccel, pkt.zAccel);
gyro = Vector3f(pkt.rollRate, pkt.pitchRate, pkt.yawRate);
velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);
location.lat = pkt.latitude * 1.0e7;
location.lng = pkt.longitude * 1.0e7;
location.alt = pkt.altitude*1.0e2;
dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw);
2015-05-22 22:37:21 -03:00
airspeed = pkt.airspeed;
airspeed_pitot = pkt.airspeed;
// auto-adjust to last_letter frame rate
uint64_t deltat_us = pkt.timestamp_us - last_timestamp_us;
time_now_us += deltat_us;
if (deltat_us < 1.0e4 && deltat_us > 0) {
adjust_frame_time(1.0e6/deltat_us);
}
last_timestamp_us = pkt.timestamp_us;
}
/*
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();
time_advance();
// update magnetic field
update_mag_field_bf();
}
2015-10-22 10:04:42 -03:00
} // namespace SITL