mirror of https://github.com/ArduPilot/ardupilot
SITL: Add scrimmage simulator support
This commit is contained in:
Anthony Velte
Andrew Tridgell
parent
92dde5b111
commit
92a9ab6c6d
|
|
@ -103,6 +103,11 @@ public:
|
|||
|
||||
float gross_mass() const { return mass + external_payload_mass; }
|
||||
|
||||
virtual void set_config(const char* config) {
|
||||
config_ = config;
|
||||
}
|
||||
|
||||
|
||||
const Location &get_location() const { return location; }
|
||||
|
||||
const Vector3f &get_position() const { return position; }
|
||||
|
|
@ -178,6 +183,7 @@ protected:
|
|||
const char *frame;
|
||||
bool use_time_sync = true;
|
||||
float last_speedup = -1.0f;
|
||||
const char *config_ = "";
|
||||
|
||||
// allow for AHRS_ORIENTATION
|
||||
AP_Int8 *ahrs_orientation;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,216 @@
|
|||
/*
|
||||
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 Scrimmage simulator
|
||||
*/
|
||||
|
||||
#include "SIM_Scrimmage.h"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <stdio.h>
|
||||
#include <inttypes.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
namespace SITL {
|
||||
|
||||
Scrimmage::Scrimmage(const char *home_str, const char *_frame_str) :
|
||||
Aircraft(home_str, _frame_str),
|
||||
prev_timestamp_us(0),
|
||||
recv_sock(true),
|
||||
send_sock(true),
|
||||
frame_str(_frame_str)
|
||||
{
|
||||
// Set defaults for scrimmage-copter
|
||||
if (strcmp(frame_str, "scrimmage-copter")== 0) {
|
||||
mission_name = "arducopter.xml";
|
||||
motion_model = "Multirotor";
|
||||
visual_model = "iris";
|
||||
}
|
||||
}
|
||||
|
||||
void Scrimmage::set_config(const char *config)
|
||||
{
|
||||
// The configuration string is a comma-separated sequence of key:value
|
||||
// pairs
|
||||
|
||||
// Iterate over comma-separated strings and store parameters
|
||||
char *end_str;
|
||||
char* copy_config = strdup(config);
|
||||
char *token = strtok_r(copy_config, ",", &end_str);
|
||||
while (token != NULL)
|
||||
{
|
||||
char *end_token;
|
||||
char *token2 = strtok_r(token, "=", &end_token);
|
||||
if (strcmp(token2, "mission")==0) {
|
||||
mission_name = strtok_r(NULL, "=", &end_token);
|
||||
} else if (strcmp(token2, "motion_model")==0) {
|
||||
motion_model = strtok_r(NULL, "=", &end_token);
|
||||
} else if (strcmp(token2, "visual_model")==0) {
|
||||
visual_model = strtok_r(NULL, "=", &end_token);
|
||||
} else if (strcmp(token2, "terrain")==0) {
|
||||
terrain = strtok_r(NULL, "=", &end_token);
|
||||
} else {
|
||||
printf("Invalid scrimmage param: %s", token2);
|
||||
}
|
||||
token = strtok_r(NULL, ",", &end_str);
|
||||
}
|
||||
free(copy_config);
|
||||
|
||||
// start scrimmage after parsing simulation configuration
|
||||
start_scrimmage();
|
||||
}
|
||||
|
||||
void Scrimmage::set_interface_ports(const char* address, const int port_in, const int port_out)
|
||||
{
|
||||
fdm_port_in = port_in;
|
||||
fdm_port_out = port_out;
|
||||
fdm_address = address;
|
||||
printf("ArduPilot sending to scrimmage on %s:%d\n",fdm_address, fdm_port_out);
|
||||
printf("ArduPilot listening to scrimmage on %s:%d\n",fdm_address, fdm_port_in);
|
||||
|
||||
recv_sock.bind(fdm_address, fdm_port_in);
|
||||
|
||||
recv_sock.reuseaddress();
|
||||
recv_sock.set_blocking(false);
|
||||
|
||||
send_sock.reuseaddress();
|
||||
send_sock.set_blocking(false);
|
||||
}
|
||||
|
||||
// Start Scrimmage child
|
||||
void Scrimmage::start_scrimmage(void)
|
||||
{
|
||||
pid_t child_pid = fork();
|
||||
if (child_pid == 0) {
|
||||
// In child
|
||||
|
||||
// Construct the scrimmage command string with overrides for initial
|
||||
// position and heading.
|
||||
char* full_exec_str;
|
||||
int len;
|
||||
// Get required string length
|
||||
len = snprintf(NULL, 0, "xterm +hold -T SCRIMMAGE -e 'scrimmage %s -o \"latitude_origin=%f,"
|
||||
"longitude_origin=%f,altitude_origin=%f,heading=%f,motion_model=%s,visual_model=%s,terrain=%s,"
|
||||
"to_ardupilot_port=%d,from_ardupilot_port=%d,to_ardupilot_ip=%s\"'", mission_name, home.lat*1.0e-7f,home.lng*1.0e-7f,
|
||||
home.alt*1.0e-2f, home_yaw, motion_model, visual_model, terrain, fdm_port_in, fdm_port_out, fdm_address);
|
||||
|
||||
full_exec_str = (char *)malloc(len+1);
|
||||
snprintf(full_exec_str, len+1, "xterm +hold -T SCRIMMAGE -e 'scrimmage %s -o \"latitude_origin=%f,"
|
||||
"longitude_origin=%f,altitude_origin=%f,heading=%f,motion_model=%s,visual_model=%s,terrain=%s,"
|
||||
"to_ardupilot_port=%d,from_ardupilot_port=%d,to_ardupilot_ip=%s\"'", mission_name, home.lat*1.0e-7f,home.lng*1.0e-7f,
|
||||
home.alt*1.0e-2f, home_yaw, motion_model, visual_model, terrain, fdm_port_in, fdm_port_out, fdm_address);
|
||||
|
||||
printf("%s\n", full_exec_str);
|
||||
|
||||
// system call worked
|
||||
int ret = system(full_exec_str);
|
||||
|
||||
if (ret != 0) {
|
||||
std::cerr << "scrimmage didn't open.\n";
|
||||
perror("scrimmage");
|
||||
}
|
||||
|
||||
free(full_exec_str);
|
||||
}
|
||||
}
|
||||
|
||||
void Scrimmage::send_servos(const struct sitl_input &input)
|
||||
{
|
||||
servo_packet pkt;
|
||||
|
||||
for (int i = 0; i < MAX_NUM_SERVOS; i++) {
|
||||
pkt.servos[i] = input.servos[i];
|
||||
}
|
||||
send_sock.sendto(&pkt, sizeof(servo_packet), fdm_address, fdm_port_out);
|
||||
}
|
||||
|
||||
/*
|
||||
receive an update from the FDM
|
||||
This is a blocking function
|
||||
*/
|
||||
void Scrimmage::recv_fdm(const struct sitl_input &input)
|
||||
{
|
||||
fdm_packet pkt;
|
||||
|
||||
// wait for packet from scrimmage
|
||||
while (recv_sock.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt));
|
||||
|
||||
// auto-adjust to simulation frame rate
|
||||
uint64_t dt_us = 0;
|
||||
if (pkt.timestamp_us > prev_timestamp_us)
|
||||
dt_us = pkt.timestamp_us - prev_timestamp_us;
|
||||
time_now_us += dt_us;
|
||||
|
||||
float dt_inv = 1.0e6 / ((float)dt_us);
|
||||
if ( dt_inv > 100) {
|
||||
adjust_frame_time(dt_inv);
|
||||
}
|
||||
prev_timestamp_us = pkt.timestamp_us;
|
||||
|
||||
// dcm_bl: dcm from body to local frame
|
||||
dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw);
|
||||
dcm.normalize();
|
||||
|
||||
// subtract gravity to get specific force measuremnt of the IMU
|
||||
accel_body = Vector3f(pkt.xAccel, pkt.yAccel, pkt.zAccel) - dcm.transposed()*Vector3f(0.0f, 0.0f, GRAVITY_MSS);
|
||||
gyro = Vector3f(pkt.rollRate, pkt.pitchRate, pkt.yawRate);
|
||||
|
||||
ang_accel = (gyro - gyro_prev) * std::min((float)1000000, dt_inv);
|
||||
gyro_prev = gyro;
|
||||
|
||||
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;
|
||||
position.z = (home.alt - location.alt) * 1.0e-2;
|
||||
|
||||
|
||||
// velocity relative to air mass, in earth frame TODO
|
||||
velocity_air_ef = velocity_ef;
|
||||
|
||||
// velocity relative to airmass in body frame TODO
|
||||
velocity_air_bf = dcm.transposed() * velocity_air_ef;
|
||||
|
||||
battery_voltage = 0;
|
||||
battery_current = 0;
|
||||
rpm1 = 0;
|
||||
rpm2 = 0;
|
||||
|
||||
airspeed = pkt.airspeed;
|
||||
airspeed_pitot = pkt.airspeed;
|
||||
}
|
||||
|
||||
/*
|
||||
update the Scrimmage simulation by one time step
|
||||
*/
|
||||
void Scrimmage::update(const struct sitl_input &input)
|
||||
{
|
||||
send_servos(input);
|
||||
recv_fdm(input);
|
||||
update_mag_field_bf();
|
||||
}
|
||||
|
||||
} // namespace SITL
|
||||
|
|
@ -0,0 +1,95 @@
|
|||
/*
|
||||
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 connection for Scrimmage Simulator
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <map>
|
||||
|
||||
#include <AP_HAL/utility/Socket.h>
|
||||
|
||||
#include "SIM_Aircraft.h"
|
||||
|
||||
namespace SITL {
|
||||
|
||||
/*
|
||||
a last_letter simulator
|
||||
*/
|
||||
class Scrimmage : public Aircraft {
|
||||
public:
|
||||
Scrimmage(const char *home_str, const char *frame_str);
|
||||
|
||||
/* update model by one time step */
|
||||
void update(const struct sitl_input &input) override;
|
||||
|
||||
/* static object creator */
|
||||
static Aircraft *create(const char *home_str, const char *frame_str) {
|
||||
return new Scrimmage(home_str, frame_str);
|
||||
}
|
||||
|
||||
void set_config(const char *config) override;
|
||||
|
||||
/* Create and set in/out socket for extenal simulator */
|
||||
void set_interface_ports(const char* address, const int port_in, const int port_out) override;
|
||||
|
||||
private:
|
||||
uint16_t fdm_port_in;
|
||||
uint16_t fdm_port_out;
|
||||
const char* fdm_address;
|
||||
|
||||
/*
|
||||
packet sent to Scrimmage from ArduPilot
|
||||
*/
|
||||
static const int MAX_NUM_SERVOS = 16;
|
||||
struct servo_packet {
|
||||
uint16_t servos[MAX_NUM_SERVOS];
|
||||
};
|
||||
|
||||
/*
|
||||
state packet sent from Scrimmage to ArduPilot
|
||||
*/
|
||||
struct fdm_packet {
|
||||
uint64_t timestamp_us; // simulation time in microseconds
|
||||
double latitude, longitude;
|
||||
double altitude;
|
||||
double heading;
|
||||
double speedN, speedE, speedD;
|
||||
double xAccel, yAccel, zAccel;
|
||||
double rollRate, pitchRate, yawRate;
|
||||
double roll, pitch, yaw;
|
||||
double airspeed;
|
||||
};
|
||||
|
||||
void recv_fdm(const struct sitl_input &input);
|
||||
void send_servos(const struct sitl_input &input);
|
||||
void start_scrimmage(void);
|
||||
|
||||
uint64_t prev_timestamp_us;
|
||||
SocketAPM recv_sock;
|
||||
SocketAPM send_sock;
|
||||
|
||||
const char *frame_str;
|
||||
|
||||
// Use ArduPlane by default
|
||||
const char *mission_name = "arduplane.xml";
|
||||
const char *motion_model = "JSBSimControl";
|
||||
const char *visual_model = "zephyr-blue";
|
||||
const char *terrain = "mcmillan";
|
||||
};
|
||||
|
||||
} // namespace SITL
|
||||
Loading…
Reference in New Issue