From 92a9ab6c6de3f62f680879a677e7661eb435962f Mon Sep 17 00:00:00 2001 From: Anthony Velte Date: Thu, 18 Jul 2019 13:36:52 -0400 Subject: [PATCH] SITL: Add scrimmage simulator support --- libraries/SITL/SIM_Aircraft.h | 6 + libraries/SITL/SIM_Scrimmage.cpp | 216 +++++++++++++++++++++++++++++++ libraries/SITL/SIM_Scrimmage.h | 95 ++++++++++++++ 3 files changed, 317 insertions(+) create mode 100644 libraries/SITL/SIM_Scrimmage.cpp create mode 100644 libraries/SITL/SIM_Scrimmage.h diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 6eddf9fd5c..f2f687c4fb 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -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; diff --git a/libraries/SITL/SIM_Scrimmage.cpp b/libraries/SITL/SIM_Scrimmage.cpp new file mode 100644 index 0000000000..1f1af6c4a7 --- /dev/null +++ b/libraries/SITL/SIM_Scrimmage.cpp @@ -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 . + */ +/* + simulator connector for Scrimmage simulator +*/ + +#include "SIM_Scrimmage.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +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 diff --git a/libraries/SITL/SIM_Scrimmage.h b/libraries/SITL/SIM_Scrimmage.h new file mode 100644 index 0000000000..160a7da86e --- /dev/null +++ b/libraries/SITL/SIM_Scrimmage.h @@ -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 . + */ +/* + simulator connection for Scrimmage Simulator +*/ + +#pragma once + +#include +#include + +#include + +#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