diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 8300f7a77..fd3b663d1 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -280,10 +280,10 @@ struct AirSimSettings { int tcp_port = 4560; // The PX4 SITL app requires receiving drone commands over a different mavlink channel called - // the "ground control station" channel. - // So set this to empty string to disable this separate command channel. + // the "ground control station" (or GCS) channel. std::string control_ip_address = "127.0.0.1"; - int control_port = 14580; + int control_port_local = 14540; + int control_port_remote = 14580; // The log viewer can be on a different machine, so you can configure it's ip address and port here. int logviewer_ip_port = 14388; @@ -311,6 +311,7 @@ struct AirSimSettings { std::string model = "Generic"; std::map params; + std::string logs; }; struct MavLinkVehicleSetting : public VehicleSetting { @@ -714,7 +715,9 @@ struct AirSimSettings { connection_info.qgc_ip_port = settings_json.getInt("QgcPort", connection_info.qgc_ip_port); connection_info.control_ip_address = settings_json.getString("ControlIp", connection_info.control_ip_address); - connection_info.control_port = settings_json.getInt("ControlPort", connection_info.control_port); + connection_info.control_port_local = settings_json.getInt("ControlPort", connection_info.control_port_local); // legacy + connection_info.control_port_local = settings_json.getInt("ControlPortLocal", connection_info.control_port_local); + connection_info.control_port_remote = settings_json.getInt("ControlPortRemote", connection_info.control_port_remote); std::string sitlip = settings_json.getString("SitlIp", connection_info.control_ip_address); if (sitlip.size() > 0 && connection_info.control_ip_address.size() == 0) @@ -725,7 +728,7 @@ struct AirSimSettings { if (settings_json.hasKey("SitlPort")) { // backwards compat - connection_info.control_port = settings_json.getInt("SitlPort", connection_info.control_port); + connection_info.control_port_local = settings_json.getInt("SitlPort", connection_info.control_port_local); } connection_info.local_host_ip = settings_json.getString("LocalHostIp", connection_info.local_host_ip); @@ -738,6 +741,7 @@ struct AirSimSettings { connection_info.serial_port = settings_json.getString("SerialPort", connection_info.serial_port); connection_info.baud_rate = settings_json.getInt("SerialBaudRate", connection_info.baud_rate); connection_info.model = settings_json.getString("Model", connection_info.model); + connection_info.logs = settings_json.getString("Logs", connection_info.logs); Settings params; if (settings_json.getChild("Parameters", params)) { diff --git a/AirLib/include/sensors/gps/GpsSimpleParams.hpp b/AirLib/include/sensors/gps/GpsSimpleParams.hpp index 52d8cf88c..43e38277a 100644 --- a/AirLib/include/sensors/gps/GpsSimpleParams.hpp +++ b/AirLib/include/sensors/gps/GpsSimpleParams.hpp @@ -12,7 +12,7 @@ namespace msr { namespace airlib { struct GpsSimpleParams { real_T eph_time_constant = 0.9f, epv_time_constant = 0.9f; real_T eph_initial = 100.0f, epv_initial = 100.0f; //initially fully diluted positions - real_T eph_final = 0.3f, epv_final = 0.4f; + real_T eph_final = 0.1f, epv_final = 0.1f; // PX4 won't converge GPS sensor fusion until we get to 10% accuracty. real_T eph_min_3d = 3.0f, eph_min_2d = 4.0f; real_T update_latency = 0.2f; //sec diff --git a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp index 37ce927c3..b3f896649 100644 --- a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp +++ b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp @@ -142,10 +142,10 @@ class ArduRoverApi : public CarApiBase { } Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); - Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); + Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); udpSocket_ = std::make_shared(); - udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port); + udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port_local); } private: diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp index 400ac408e..2f7d1992f 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp @@ -318,10 +318,10 @@ class ArduCopterApi : public MultirotorApiBase { } Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); - Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); + Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); udpSocket_ = std::make_shared(); - udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port); + udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port_local); } private: diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp index 6190c86ef..bc1db991f 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp @@ -113,9 +113,9 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi } Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); - Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); + Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); - udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.control_port); + udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.control_port_local); mavlinkcom::AdHocMessageHandler handler = [this](std::shared_ptr connection, const std::vector &msg) { this->rotorPowerMessageHandler(connection, msg); }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp index 4b9c625f9..dd9912cdd 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp @@ -20,6 +20,7 @@ #include #include "common/Common.hpp" +#include "common/common_utils/FileSystem.hpp" #include "common/common_utils/SmoothingFilter.hpp" #include "common/common_utils/Timer.hpp" #include "common/CommonStructs.hpp" @@ -81,11 +82,30 @@ class MavLinkMultirotorApi : public MultirotorApiBase //reset PX4 stack virtual void resetImplementation() override { + // note this is called AFTER "initialize" when we've connected to the drone + // so this method cannot reset any of that connection state. MultirotorApiBase::resetImplementation(); - - resetState(); was_reset_ = true; - setNormalMode(); + } + + unsigned long long getSimTime() { + // This ensures HIL_SENSOR and HIL_GPS have matching clocks. + //if (lock_step_enabled_) { + if (sim_time_us_ == 0) { + sim_time_us_ = clock()->nowNanos() / 1000; + } + return sim_time_us_; + // } + //else { + // return clock()->nowNanos() / 1000; + //} + } + + void advanceTime() { + if (lock_step_enabled_) { + // sim_time_us_ += ticks_per_update_; + } + sim_time_us_ = clock()->nowNanos() / 1000; } //update sensors in PX4 stack @@ -97,10 +117,16 @@ class MavLinkMultirotorApi : public MultirotorApiBase if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_) return; - if (send_params_) { - send_params_ = false; - sendParams(); - } + //auto now = clock()->nowNanos() / 1000; + + //if (last_hil_sensor_time_ != 0 && last_hil_sensor_time_ + ticks_per_update_ > now) + //{ + // // then update() is being called too often, so we just skip this one. + // // TODO: I think this needs to be aware of AirSim ClockSpeed... + // // return; + //} + + advanceTime(); //send sensor updates const auto& imu_output = getImuData(""); @@ -111,7 +137,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase imu_output.angular_velocity, mag_output.magnetic_field_body, baro_output.pressure * 0.01f /*Pa to Millibar */, baro_output.altitude); - + + sendSystemTime(); const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance); if (count_distance_sensors != 0) { @@ -146,7 +173,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog, gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10); } - } + } + } catch (std::exception& e) { addStatusMessage("Exception sending messages to vehicle"); @@ -273,6 +301,48 @@ class MavLinkMultirotorApi : public MultirotorApiBase return rc; } + void onArmed() { + if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) { + auto con = mav_vehicle_->getConnection(); + if (con != nullptr) { + if (log_ != nullptr) { + log_->close(); + } + + std::time_t t = std::time(0); // get time now + std::tm* now = std::localtime(&t); + auto folder = Utils::stringf("%04d-%02d-%02d", now->tm_year + 1900, now->tm_mon + 1, now->tm_mday); + auto path = common_utils::FileSystem::ensureFolder(connection_info_.logs, folder); + auto filename = Utils::stringf("%02d-%02d-%02d.mavlink", now->tm_hour, now->tm_min, now->tm_sec); + auto fullpath = common_utils::FileSystem::combine(path, filename); + addStatusMessage(Utils::stringf("Opening log file: %s", fullpath.c_str())); + log_ = std::make_shared(); + log_->openForWriting(fullpath, false); + con->startLoggingSendMessage(log_); + con->startLoggingReceiveMessage(log_); + if (con != connection_) { + // log the SITL channel also + connection_->startLoggingSendMessage(log_); + connection_->startLoggingReceiveMessage(log_); + } + } + } + } + + void onDisarmed() { + if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) { + auto con = mav_vehicle_->getConnection(); + if (con != nullptr) { + con->stopLoggingSendMessage(); + addStatusMessage("Closing log file."); + } + connection_->stopLoggingSendMessage(); + if (log_ != nullptr) { + log_->close(); + } + } + } + void waitForHomeLocation(float timeout_sec) { if (!current_state_.home.is_set) { @@ -712,7 +782,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase if (qgc_proxy_ != nullptr) { qgc_proxy_->close(); qgc_proxy_ = nullptr; - } + } + + resetState(); } void connect_thread() @@ -724,8 +796,12 @@ class MavLinkMultirotorApi : public MultirotorApiBase connectToLogViewer(); connectToQGC(); } - connecting_ = false; - connected_ = true; + + if (connecting_) { + // only set connected if we haven't already been told to disconnect. + connecting_ = false; + connected_ = true; + } } virtual void close() @@ -743,9 +819,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase void openAllConnections() { + Utils::log("Opening mavlink connection"); close(); //just in case if connections were open resetState(); //reset all variables we might have changed during last session - connect(); } @@ -1034,11 +1110,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase close(); connecting_ = true; - got_first_heartbeat_ = false; - is_hil_mode_set_ = false; - is_armed_ = false; - has_home_ = false; - is_controls_0_1_ = true; + is_controls_0_1_ = true; + std::string remoteIpAddr; Utils::setValue(rotor_controls_, 0.0f); if (connection_info.use_tcp) { @@ -1050,7 +1123,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase addStatusMessage(msg); try { connection_ = std::make_shared(); - connection_->acceptTcp("hil", connection_info_.local_host_ip, connection_info.tcp_port); + remoteIpAddr = connection_->acceptTcp("hil", connection_info_.local_host_ip, connection_info.tcp_port); } catch (std::exception& e) { addStatusMessage("Accepting TCP socket failed, is another instance running?"); @@ -1088,28 +1161,52 @@ class MavLinkMultirotorApi : public MultirotorApiBase mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); if (connection_info_.control_ip_address != "") { - if (connection_info_.control_port == 0) { - throw std::invalid_argument("ControlPort setting has an invalid value."); + if (connection_info_.control_port_local == 0) { + throw std::invalid_argument("LocalControlPort setting has an invalid value."); + } + if (!connection_info.use_tcp || connection_info_.control_ip_address != "remote") { + remoteIpAddr = connection_info_.control_ip_address; + } + if (remoteIpAddr == "local" || remoteIpAddr == "localhost") { + remoteIpAddr = "127.0.0.1"; } // The PX4 SITL mode app cannot receive commands to control the drone over the same HIL mavlink connection. // The HIL mavlink connection can only handle HIL_SENSOR messages. This separate channel is needed for // everything else. - addStatusMessage(Utils::stringf("Connecting to PX4 Control UDP port %d, local IP %s, remote IP...", - connection_info_.control_port, connection_info_.local_host_ip.c_str(), connection_info_.control_ip_address.c_str())); + addStatusMessage(Utils::stringf("Connecting to PX4 Control UDP port %d, local IP %s, remote IP %s ...", + connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), remoteIpAddr.c_str())); // if we try and connect the UDP port too quickly it doesn't work, bug in PX4 ? for (int retries = 60; retries >= 0 && connecting_; retries--) { try { - auto gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs", - connection_info_.local_host_ip, connection_info_.control_ip_address, connection_info_.control_port); + std::shared_ptr gcsConnection; + if (remoteIpAddr == "127.0.0.1") { + gcsConnection = mavlinkcom::MavLinkConnection::connectLocalUdp("gcs", + connection_info_.local_host_ip, + connection_info_.control_port_local); + } + else { + gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs", + connection_info_.local_host_ip, + remoteIpAddr, + connection_info_.control_port_remote); + } mav_vehicle_->connect(gcsConnection); + // need to try and send something to make sure the connection is good. + mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1); + break; } catch (std::exception&) { std::this_thread::sleep_for(std::chrono::seconds(1)); } } + if (mav_vehicle_ == nullptr) { + // play was stopped! + return; + } + if (mav_vehicle_->getConnection() != nullptr) { addStatusMessage(std::string("Ground control connected over UDP.")); } @@ -1117,31 +1214,73 @@ class MavLinkMultirotorApi : public MultirotorApiBase addStatusMessage(std::string("Timeout trying to connect ground control over UDP.")); return; } + } + try { + connectVehicle(); + } + catch (std::exception& e) { + addStatusMessage("Error connecting vehicle:"); + addStatusMessage(e.what()); } + } - connectVehicle(); + void processControlMessages(const mavlinkcom::MavLinkMessage& msg) + { + // Utils::log(Utils::stringf("Control msg %d", msg.msgid)); + // PX4 usually sends the following on the control channel. + // If nothing is arriving here it means our control channel UDP connection isn't working. + // MAVLINK_MSG_ID_HIGHRES_IMU + // MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET + // MAVLINK_MSG_ID_SERVO_OUTPUT_RAW + // MAVLINK_MSG_ID_GPS_RAW_INT + // MAVLINK_MSG_ID_TIMESYNC + // MAVLINK_MSG_ID_ALTITUDE + // MAVLINK_MSG_ID_VFR_HUD + // MAVLINK_MSG_ID_ESTIMATOR_STATUS + // MAVLINK_MSG_ID_EXTENDED_SYS_STATE + // MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN + // MAVLINK_MSG_ID_PING + // MAVLINK_MSG_ID_SYS_STATUS + // MAVLINK_MSG_ID_SYSTEM_TIME + // MAVLINK_MSG_ID_LINK_NODE_STATUS + // MAVLINK_MSG_ID_AUTOPILOT_VERSION + // MAVLINK_MSG_ID_COMMAND_ACK + // MAVLINK_MSG_ID_STATUSTEXT + // MAVLINK_MSG_ID_PARAM_VALUE + processMavMessages(msg); } void connectVehicle() { // listen to this UDP mavlink connection also auto mavcon = mav_vehicle_->getConnection(); - if (mavcon != connection_) { + if (mavcon != nullptr && mavcon != connection_) { mavcon->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { - unused(connection); - processMavMessages(msg); - }); + unused(connection); + processControlMessages(msg); + }); } else { mav_vehicle_->connect(connection_); } connected_ = true; - // now we can start our heartbeats. - mav_vehicle_->startHeartbeat(); // Also request home position messages mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1); + + // now we can start our heartbeats. + mav_vehicle_->startHeartbeat(); + + // wait for px4 to connect so we can safely send any configured parameters + while (!send_params_ && connected_) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + if (connected_) { + sendParams(); + send_params_ = false; + } } void createMavSerialConnection(const std::string& port_name, int baud_rate) @@ -1184,15 +1323,15 @@ class MavLinkMultirotorApi : public MultirotorApiBase hil_node_->connect(connection_); addStatusMessage(Utils::stringf("Connected to PX4 over serial port: %s", port_name_auto.c_str())); - mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); - mav_vehicle_->connect(connection_); // in this case we can use the same connection. - mav_vehicle_->startHeartbeat(); // start listening to the HITL connection. connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { unused(connection); processMavMessages(msg); }); + mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); + + connectVehicle(); return; } catch (std::exception& e) { @@ -1228,24 +1367,41 @@ class MavLinkMultirotorApi : public MultirotorApiBase void sendParams() { // send any mavlink parameters from settings.json through to the connected vehicle. - if (connection_info_.params.size() > 0) { - for (auto iter : connection_info_.params) { - auto key = iter.first; - auto value = iter.second; - mavlinkcom::MavLinkParameter p; - p.name = key; - p.value = value; - bool result = false; - mav_vehicle_->setParameter(p).wait(1000, &result); - if (!result) { - Utils::log(Utils::stringf("Failed to set mavlink parameter '%s'", key.c_str())); + if (mav_vehicle_ != nullptr && connection_info_.params.size() > 0) { + try { + for (auto iter : connection_info_.params) { + auto key = iter.first; + auto value = iter.second; + mavlinkcom::MavLinkParameter p; + p.name = key; + p.value = value; + bool result = false; + mav_vehicle_->setParameter(p).wait(1000, &result); + if (!result) { + Utils::log(Utils::stringf("Failed to set mavlink parameter '%s'", key.c_str())); + } } } + catch (std::exception& ex) { + addStatusMessage("Exception sending parameters to vehicle"); + addStatusMessage(ex.what()); + } } } void setArmed(bool armed) { + if (is_armed_) { + if (!armed) { + onDisarmed(); + } + } + else { + if (armed) { + onArmed(); + } + } + is_armed_ = armed; if (!armed) { //reset motor controls @@ -1296,8 +1452,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase // and it scales multi rotor servo output to 0 to 1. is_controls_0_1_ = false; } - - send_params_ = true; } else if (is_simulation_mode_ && !is_hil_mode_set_) { setHILMode(); @@ -1376,7 +1530,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase addStatusMessage("Got GPS Home Location"); has_home_ = true; } - + send_params_ = true; } else if (msg.msgid == mavlinkcom::MavLinkLocalPositionNed::kMessageId) { // we are getting position information... so we can use this to check the stability of the z coordinate before takeoff. @@ -1388,20 +1542,34 @@ class MavLinkMultirotorApi : public MultirotorApiBase else if (msg.msgid == mavlinkcom::MavLinkExtendedSysState::kMessageId) { // check landed state. getLandedState(); + send_params_ = true; } else if (msg.msgid == mavlinkcom::MavLinkHomePosition::kMessageId) { mavlinkcom::MavLinkHomePosition home; home.decode(msg); + // this is a good time to send the params + send_params_ = true; + } + else if (msg.msgid == mavlinkcom::MavLinkSysStatus::kMessageId) { + // this is a good time to send the params + send_params_ = true; + } + else if (msg.msgid == mavlinkcom::MavLinkAutopilotVersion::kMessageId) { + // this is a good time to send the params + send_params_ = true; + } + else { + // creates too much log output, only do this when debugging this issue specifically. + // Utils::log(Utils::stringf("Ignornig msg %d from %d,%d ", msg.msgid, msg.compid, msg.sysid)); } - //else ignore message } void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro, const Vector3r& mag, float abs_pressure, float pressure_alt) { if (!is_simulation_mode_) throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode"); - - auto now = clock()->nowNanos() / 1000; + + auto now = getSimTime(); if (lock_step_enabled_) { if (last_hil_sensor_time_ + 100000 < now) { // if 100 ms passes then something is terribly wrong, reset lockstep mode @@ -1457,14 +1625,28 @@ class MavLinkMultirotorApi : public MultirotorApiBase last_sensor_message_ = hil_sensor; } + void sendSystemTime() { + // SYSTEM TIME from host + auto tu = getSimTime(); + uint32_t ms = (uint32_t)(tu / 1000); + if (ms != last_sys_time_) { + last_sys_time_ = ms; + mavlinkcom::MavLinkSystemTime msg_system_time; + msg_system_time.time_unix_usec = tu; + msg_system_time.time_boot_ms = last_sys_time_; + if (hil_node_ != nullptr) { + hil_node_->sendMessage(msg_system_time); + } + } + } + void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, Quaternionr orientation) { if (!is_simulation_mode_) throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode"); mavlinkcom::MavLinkDistanceSensor distance_sensor; - - distance_sensor.time_boot_ms = static_cast(clock()->nowNanos() / 1000000); + distance_sensor.time_boot_ms = static_cast(getSimTime() / 1000); distance_sensor.min_distance = static_cast(min_distance); distance_sensor.max_distance = static_cast(max_distance); distance_sensor.current_distance = static_cast(current_distance); @@ -1491,6 +1673,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase void sendHILGps(const GeoPoint& geo_point, const Vector3r& velocity, float velocity_xy, float cog, float eph, float epv, int fix_type, unsigned int satellites_visible) { + unused(satellites_visible); + if (!is_simulation_mode_) throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode"); @@ -1529,10 +1713,17 @@ class MavLinkMultirotorApi : public MultirotorApiBase is_controls_0_1_ = true; hil_state_freq_ = -1; actuators_message_supported_ = false; - last_gps_time_ = 0; state_version_ = 0; current_state_ = mavlinkcom::VehicleState(); target_height_ = 0; + got_first_heartbeat_ = false; + is_armed_ = false; + has_home_ = false; + sim_time_us_ = 0; + last_sys_time_ = 0; + last_gps_time_ = 0; + last_hil_sensor_time_ = 0; + hil_sensor_clock_ = 0; is_api_control_enabled_ = false; thrust_controller_ = PidController(); Utils::setValue(rotor_controls_, 0.0f); @@ -1607,15 +1798,12 @@ class MavLinkMultirotorApi : public MultirotorApiBase std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, last_message_mutex_; //variables required for VehicleApiBase implementation - bool got_first_heartbeat_, is_hil_mode_set_, is_armed_; + bool got_first_heartbeat_ = false, is_hil_mode_set_ = false, is_armed_ = false; bool is_controls_0_1_; //Are motor controls specified in 0..1 or -1..1? bool send_params_ = false; std::queue status_messages_; int hil_state_freq_; bool actuators_message_supported_ = false; - uint64_t last_gps_time_ = 0; - uint64_t last_hil_sensor_time_ = 0; - uint64_t hil_sensor_clock_ = 0; bool was_reset_ = false; bool has_home_ = false; bool is_ready_ = false; @@ -1631,6 +1819,15 @@ class MavLinkMultirotorApi : public MultirotorApiBase double ground_variance_ = 1; const double GroundTolerance = 0.1; + // variables for throttling HIL_SENSOR and SYSTEM_TIME messages + const int DEFAULT_SIM_RATE = 250; // Hz. + const unsigned long ticks_per_update_ = 1000000 / DEFAULT_SIM_RATE; // microseconds + uint32_t last_sys_time_ = 0; + unsigned long long sim_time_us_ = 0; + uint64_t last_gps_time_ = 0; + uint64_t last_hil_sensor_time_ = 0; + uint64_t hil_sensor_clock_ = 0; + //additional variables required for MultirotorApiBase implementation //this is optional for methods that might not use vehicle commands std::shared_ptr mav_vehicle_; @@ -1639,6 +1836,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase PidController thrust_controller_; common_utils::Timer hil_message_timer_; common_utils::Timer gcs_message_timer_; + std::shared_ptr log_; //every time we return status update, we need to check if we have new data //this is why below two variables are marked as mutable diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp index b64000cc4..1806ec60c 100644 --- a/DroneServer/main.cpp +++ b/DroneServer/main.cpp @@ -58,7 +58,9 @@ int main(int argc, const char* argv[]) connection_info.qgc_ip_port = child.getInt("QgcPort", connection_info.qgc_ip_port); connection_info.control_ip_address = child.getString("ControlIp", connection_info.control_ip_address); - connection_info.control_port = child.getInt("ControlPort", connection_info.control_port); + connection_info.control_port_local = child.getInt("ControlPort", connection_info.control_port_local); + connection_info.control_port_local = child.getInt("ControlPortLocal", connection_info.control_port_local); + connection_info.control_port_remote = child.getInt("ControlPortRemote", connection_info.control_port_remote); connection_info.local_host_ip = child.getString("LocalHostIp", connection_info.local_host_ip); diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj index d150fadca..7b61e1a5d 100644 --- a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj +++ b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj @@ -117,7 +117,7 @@ - Use + NotUsing Level3 Disabled true @@ -131,7 +131,7 @@ - Use + NotUsing Level3 MaxSpeed true diff --git a/MavLinkCom/MavLinkCom.vcxproj b/MavLinkCom/MavLinkCom.vcxproj index 319db2dad..71c5a5fad 100644 --- a/MavLinkCom/MavLinkCom.vcxproj +++ b/MavLinkCom/MavLinkCom.vcxproj @@ -380,6 +380,7 @@ + diff --git a/MavLinkCom/MavLinkCom.vcxproj.filters b/MavLinkCom/MavLinkCom.vcxproj.filters index 0cc6ffdba..fcaa6c57d 100644 --- a/MavLinkCom/MavLinkCom.vcxproj.filters +++ b/MavLinkCom/MavLinkCom.vcxproj.filters @@ -218,6 +218,7 @@ + diff --git a/MavLinkCom/common_utils/Utils.hpp b/MavLinkCom/common_utils/Utils.hpp index 66f27b6d3..7c0bd91c8 100644 --- a/MavLinkCom/common_utils/Utils.hpp +++ b/MavLinkCom/common_utils/Utils.hpp @@ -22,6 +22,7 @@ #include #include #include "type_utils.hpp" +#include "MavLinkDebugLog.hpp" #ifndef _WIN32 #include // needed for CHAR_BIT used below @@ -98,17 +99,6 @@ class Utils { public: - class Logger { - public: - virtual void log(int level, const std::string& message) - { - if (level >= 0) - std::cout << message; - else - std::cerr << message; - } - }; - static void enableImmediateConsoleFlush() { //disable buffering setbuf(stdout, NULL); @@ -136,25 +126,12 @@ class Utils { return static_cast(radians * 180.0f / M_PIf); } - static Logger* getSetLogger(Logger* logger = nullptr) - { - static Logger logger_default_; - static Logger* logger_; - - if (logger != nullptr) - logger_ = logger; - else if (logger_ == nullptr) - logger_ = &logger_default_; - - return logger_; - } - static constexpr int kLogLevelInfo = 0; static constexpr int kLogLevelWarn = -1; static constexpr int kLogLevelError = -2; static void log(std::string message, int level = kLogLevelInfo) { - getSetLogger()->log(level, message); + mavlinkcom::MavLinkDebugLog::getSetLogger()->log(level, message); } template diff --git a/MavLinkCom/include/AsyncResult.hpp b/MavLinkCom/include/AsyncResult.hpp index 0bb6df72e..64fb1ba31 100644 --- a/MavLinkCom/include/AsyncResult.hpp +++ b/MavLinkCom/include/AsyncResult.hpp @@ -30,7 +30,8 @@ namespace mavlinkcom { ~AsyncResultState() { complete(); - } + } + int getState() { return state_; } @@ -83,6 +84,12 @@ namespace mavlinkcom { state_ = nullptr; } + static AsyncResult Completed(T state) { + AsyncResult r(nullptr); + r.setResult(state); + return r; + } + void then(ResultHandler handler) { // keep state alive while we wait for async result. diff --git a/MavLinkCom/include/MavLinkConnection.hpp b/MavLinkCom/include/MavLinkConnection.hpp index 6dc7cd3e0..c77693b77 100644 --- a/MavLinkCom/include/MavLinkConnection.hpp +++ b/MavLinkCom/include/MavLinkConnection.hpp @@ -89,7 +89,8 @@ namespace mavlinkcom { // You may need to open this port in your firewall. // The localAddr can also a specific local ip address if you need to specify which // NIC to use, for example, wifi versus hard wired ethernet adapter. For localhost pass 127.0.0.1. - void acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort); + // It returns the address of the remote machine that connected. + std::string acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort); // instance methods std::string getName(); @@ -106,6 +107,10 @@ namespace mavlinkcom { void startLoggingSendMessage(std::shared_ptr log); void stopLoggingSendMessage(); + // log every message that is "received" by a subscriber. + void startLoggingReceiveMessage(std::shared_ptr log); + void stopLoggingReceiveMessage(); + uint8_t getNextSequence(); // Advanced method that create a bridge between two connections. For example, if you use connectRemoteUdp to connect to diff --git a/MavLinkCom/include/MavLinkDebugLog.hpp b/MavLinkCom/include/MavLinkDebugLog.hpp new file mode 100644 index 000000000..4dada3d66 --- /dev/null +++ b/MavLinkCom/include/MavLinkDebugLog.hpp @@ -0,0 +1,37 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef mavlink_Logger_hpp +#define mavlink_Logger_hpp + +#include +#include + +namespace mavlinkcom +{ + class MavLinkDebugLog { + public: + virtual void log(int level, const std::string& message) + { + if (level >= 0) + std::cout << message; + else + std::cerr << message; + } + + static MavLinkDebugLog* getSetLogger(MavLinkDebugLog* logger = nullptr) + { + static MavLinkDebugLog logger_default_; + static MavLinkDebugLog* logger_; + + if (logger != nullptr) + logger_ = logger; + else if (logger_ == nullptr) + logger_ = &logger_default_; + + return logger_; + } + }; +} + +#endif \ No newline at end of file diff --git a/MavLinkCom/include/MavLinkNode.hpp b/MavLinkCom/include/MavLinkNode.hpp index 54ee613d0..be754090e 100644 --- a/MavLinkCom/include/MavLinkNode.hpp +++ b/MavLinkCom/include/MavLinkNode.hpp @@ -92,7 +92,7 @@ namespace mavlinkcom { // passing a message along). void sendMessage(MavLinkMessage& msg); - // send a command to the remote node + // send a command to the remote node void sendCommand(MavLinkCommand& cmd); // send a command to the remote node and get async result that tells you whether it succeeded or not. diff --git a/MavLinkCom/include/MavLinkVehicle.hpp b/MavLinkCom/include/MavLinkVehicle.hpp index 81902f83e..9607041bb 100644 --- a/MavLinkCom/include/MavLinkVehicle.hpp +++ b/MavLinkCom/include/MavLinkVehicle.hpp @@ -37,7 +37,7 @@ namespace mavlinkcom { AsyncResult setMissionMode(); AsyncResult waitForHomePosition(); AsyncResult allowFlightControlOverUsb(); - AsyncResult setMode(int modeFlags, int customMode = 0, int customSubMode = 0); + AsyncResult setMode(int modeFlags, int customMode = 0, int customSubMode = 0, bool waitForAck = true); // wait for drone to reach specified local z, ensure it is not just blowing past the z location, // wait for it to settle down with dz delta, and dvz delta velocity. diff --git a/MavLinkCom/src/MavLinkConnection.cpp b/MavLinkCom/src/MavLinkConnection.cpp index 0d1b93ee9..27070f494 100644 --- a/MavLinkCom/src/MavLinkConnection.cpp +++ b/MavLinkCom/src/MavLinkConnection.cpp @@ -32,9 +32,9 @@ std::shared_ptr MavLinkConnection::connectTcp(const std::str return MavLinkConnectionImpl::connectTcp(nodeName, localAddr, remoteIpAddr, remotePort); } -void MavLinkConnection::acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort) +std::string MavLinkConnection::acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort) { - pImpl->acceptTcp(shared_from_this(), nodeName, localAddr, listeningPort); + return pImpl->acceptTcp(shared_from_this(), nodeName, localAddr, listeningPort); } void MavLinkConnection::startListening(const std::string& nodeName, std::shared_ptr connectedPort) @@ -53,6 +53,17 @@ void MavLinkConnection::stopLoggingSendMessage() pImpl->stopLoggingSendMessage(); } +// log every message that is "received" +void MavLinkConnection::startLoggingReceiveMessage(std::shared_ptr log) +{ + pImpl->startLoggingReceiveMessage(log); +} + +void MavLinkConnection::stopLoggingReceiveMessage() +{ + pImpl->stopLoggingReceiveMessage(); +} + void MavLinkConnection::close() { pImpl->close(); diff --git a/MavLinkCom/src/MavLinkVehicle.cpp b/MavLinkCom/src/MavLinkVehicle.cpp index 3d4a18340..8ab9fd304 100644 --- a/MavLinkCom/src/MavLinkVehicle.cpp +++ b/MavLinkCom/src/MavLinkVehicle.cpp @@ -50,10 +50,10 @@ AsyncResult MavLinkVehicle::returnToHome() return ptr->returnToHome(); } -AsyncResult MavLinkVehicle::setMode(int modeFlags, int customMode, int customSubMode) +AsyncResult MavLinkVehicle::setMode(int modeFlags, int customMode, int customSubMode, bool waitForAck) { auto ptr = static_cast(pImpl.get()); - return ptr->setMode(modeFlags, customMode, customSubMode); + return ptr->setMode(modeFlags, customMode, customSubMode, waitForAck); } bool MavLinkVehicle::isLocalControlSupported() diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp index 2e4655b7b..1202aa1c4 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp @@ -86,7 +86,7 @@ std::shared_ptr MavLinkConnectionImpl::connectTcp(const std: return createConnection(nodeName, socket); } -void MavLinkConnectionImpl::acceptTcp(std::shared_ptr parent, const std::string& nodeName, const std::string& localAddr, int listeningPort) +std::string MavLinkConnectionImpl::acceptTcp(std::shared_ptr parent, const std::string& nodeName, const std::string& localAddr, int listeningPort) { std::string local = localAddr; close(); @@ -95,10 +95,13 @@ void MavLinkConnectionImpl::acceptTcp(std::shared_ptr parent, port = socket; // this is so that a call to close() can cancel this blocking accept call. socket->accept(localAddr, listeningPort); + std::string remote = socket->remoteAddress(); + socket->setNonBlocking(); socket->setNoDelay(); parent->startListening(nodeName, socket); + return remote; } std::shared_ptr MavLinkConnectionImpl::connectSerial(const std::string& nodeName, const std::string& portName, int baudRate, const std::string& initString) @@ -144,6 +147,17 @@ void MavLinkConnectionImpl::stopLoggingSendMessage() sendLog_ = nullptr; } +// log every message that is "sent" using sendMessage. +void MavLinkConnectionImpl::startLoggingReceiveMessage(std::shared_ptr log) +{ + receiveLog_ = log; +} + +void MavLinkConnectionImpl::stopLoggingReceiveMessage() +{ + receiveLog_ = nullptr; +} + void MavLinkConnectionImpl::close() { closed = true; @@ -159,7 +173,8 @@ void MavLinkConnectionImpl::close() msg_available_.post(); publish_thread_.join(); } - sendLog_ = nullptr; + sendLog_ = nullptr; + receiveLog_ = nullptr; } bool MavLinkConnectionImpl::isOpen() @@ -502,7 +517,12 @@ void MavLinkConnectionImpl::drainQueue() if (!hasMsg) { return; + } + + if (receiveLog_ != nullptr) { + receiveLog_->write(message); } + // publish the message from this thread, this is safer than publishing from the readPackets thread // as it ensures we don't lose messages if the listener is slow. if (snapshot_stale) { diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp index 3f4325167..3256ae41d 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp @@ -36,7 +36,7 @@ namespace mavlinkcom_impl { static std::shared_ptr connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort); static std::shared_ptr connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort); static std::shared_ptr connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort); - void acceptTcp(std::shared_ptr parent, const std::string& nodeName, const std::string& localAddr, int listeningPort); + std::string acceptTcp(std::shared_ptr parent, const std::string& nodeName, const std::string& localAddr, int listeningPort); std::string getName(); int getTargetComponentId(); @@ -45,6 +45,8 @@ namespace mavlinkcom_impl { void startListening(std::shared_ptr parent, const std::string& nodeName, std::shared_ptr connectedPort); void startLoggingSendMessage(std::shared_ptr log); void stopLoggingSendMessage(); + void startLoggingReceiveMessage(std::shared_ptr log); + void stopLoggingReceiveMessage(); void close(); bool isOpen(); void sendMessage(const MavLinkMessageBase& msg); @@ -74,6 +76,7 @@ namespace mavlinkcom_impl { std::string accept_node_name_; std::shared_ptr server_; std::shared_ptr sendLog_; + std::shared_ptr receiveLog_; struct MessageHandlerEntry { public: diff --git a/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp b/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp index 43cf5097f..be5067fa6 100644 --- a/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp @@ -585,9 +585,8 @@ void MavLinkVehicleImpl::releaseControl() control_requested_ = false; control_request_sent_ = false; vehicle_state_.controls.offboard = false; - MavCmdNavGuidedEnable cmd{}; - cmd.Enable = 0; - sendCommand(cmd); + setMode(static_cast(MAV_MODE_FLAG::MAV_MODE_FLAG_CUSTOM_MODE_ENABLED), + static_cast(PX4_CUSTOM_MAIN_MODE_POSCTL), 0, false); } void MavLinkVehicleImpl::checkOffboard() @@ -609,15 +608,14 @@ void MavLinkVehicleImpl::checkOffboard() offboardIdle(); } - Utils::log("MavLinkVehicleImpl::checkOffboard: sending MavCmdNavGuidedEnable \n"); - // now the command should succeed. - bool r = false; - MavCmdNavGuidedEnable cmd{}; - cmd.Enable = 1; + Utils::log("MavLinkVehicleImpl::checkOffboard: sending MavCmdNavGuidedEnable \n"); // Note: we can't wait for ACK here, I've tried it. The ACK takes too long to get back to // us by which time the PX4 times out offboard mode!! - sendCommand(cmd); - control_request_sent_ = true; + setMode(static_cast(MAV_MODE_FLAG::MAV_MODE_FLAG_CUSTOM_MODE_ENABLED), + static_cast(PX4_CUSTOM_MAIN_MODE_OFFBOARD), 0, false); + control_request_sent_ = true; + // assume this was successful, we'll find out if so in the next heartbeat. + vehicle_state_.controls.offboard = true; } } @@ -679,11 +677,13 @@ void MavLinkVehicleImpl::moveToGlobalPosition(float lat, float lon, float alt, b writeMessage(msg); } -AsyncResult MavLinkVehicleImpl::setMode(int mode, int customMode, int customSubMode) +AsyncResult MavLinkVehicleImpl::setMode(int mode, int customMode, int customSubMode, bool waitForAck) { // this mode change take precedence over offboard mode. - control_requested_ = false; - control_request_sent_ = false; + if (customMode != static_cast(PX4_CUSTOM_MAIN_MODE_OFFBOARD)) { + control_requested_ = false; + control_request_sent_ = false; + } if ((vehicle_state_.mode & static_cast(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED)) != 0) { mode |= static_cast(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); // must preserve this flag. @@ -696,7 +696,13 @@ AsyncResult MavLinkVehicleImpl::setMode(int mode, int customMode, int cust cmd.Mode = static_cast(mode); cmd.CustomMode = static_cast(customMode); cmd.CustomSubmode = static_cast(customSubMode); - return sendCommandAndWaitForAck(cmd); + if (waitForAck) { + return sendCommandAndWaitForAck(cmd); + } + else { + sendCommand(cmd); + return AsyncResult::Completed(true); + } } AsyncResult MavLinkVehicleImpl::setPositionHoldMode() diff --git a/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp b/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp index 57ab49a43..23b7b9ce2 100644 --- a/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp +++ b/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp @@ -39,7 +39,7 @@ namespace mavlinkcom_impl { AsyncResult waitForHomePosition(); AsyncResult allowFlightControlOverUsb(); AsyncResult waitForAltitude(float z, float dz, float dvz); - AsyncResult setMode(int modeFlags, int customMode = 0, int customSubMode = 0); + AsyncResult setMode(int modeFlags, int customMode = 0, int customSubMode = 0, bool waitForAck = true); // request OFFBOARD control. void requestControl(); diff --git a/PythonClient/multirotor/stability_test.py b/PythonClient/multirotor/stability_test.py new file mode 100644 index 000000000..cf1530194 --- /dev/null +++ b/PythonClient/multirotor/stability_test.py @@ -0,0 +1,83 @@ +import os +import setup_path +import airsim +import time +import numpy as np +import sys + +script_dir = os.path.dirname(__file__) + +client = airsim.MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) + +def play_sound(wavfile): + import speaker + import wav_reader + reader = wav_reader.WavReader() + reader.open(wavfile, 512, speaker.Speaker()) + while True: + buffer = reader.read() + if buffer is None: + break + +class Numbers: + def __init__(self, name): + self.data = [] + self.name = name + + def add(self, x): + self.data += [x] + + def is_unstable(self, amount): + a = np.array(self.data) + minimum = a.min() + maximum = a.max() + mean = np.mean(a) + stddev = np.std(a) + print("{}: min={}, max={}, mean={}, stddev={}".format(self.name, minimum, maximum, mean, stddev)) + return (maximum - minimum) > amount + +while True: + x = Numbers("x") + y = Numbers("y") + z = Numbers("z") + + print("arming the drone...") + client.armDisarm(True) + + while client.getMultirotorState().landed_state == airsim.LandedState.Landed: + print("taking off...") + client.takeoffAsync().join() + time.sleep(1) + + # fly for a minute + start = time.time() + while time.time() < start + 20: + state = client.getMultirotorState() + x_val = state.kinematics_estimated.position.x_val + y_val = state.kinematics_estimated.position.y_val + z_val = state.kinematics_estimated.position.z_val + x.add(x_val) + y.add(y_val) + z.add(z_val) + print("x: {}, y: {}, z: {}".format(x_val, y_val, z_val)) + time.sleep(1) + + print("landing...") + client.landAsync().join() + + print("disarming the drone...") + client.armDisarm(False) + + # more than 50 centimeter drift is unacceptable. + a = x.is_unstable(0.5) + b = y.is_unstable(0.5) + c = z.is_unstable(0.5) + + if a or b or c: + play_sound(os.path.join(script_dir, "Error.wav")) + + time.sleep(5) + + diff --git a/build.cmd b/build.cmd index 82682d8f2..61f798ce7 100644 --- a/build.cmd +++ b/build.cmd @@ -23,15 +23,31 @@ if "%VisualStudioVersion%" lss "16.0" ( if "%1"=="" goto noargs if "%1"=="--no-full-poly-car" set "noFullPolyCar=y" -if "%1"=="--Debug" set "buildMode=--Debug" -if "%1"=="--Release" set "buildMode=--Release" +if "%1"=="--Debug" set "buildMode=Debug" +if "%1"=="--Release" set "buildMode=Release" +if "%1"=="--RelWithDebInfo" set "buildMode=RelWithDebInfo" if "%2"=="" goto noargs -if "%2"=="--Debug" set "buildMode=--Debug" -if "%2"=="--Release" set "buildMode=--Release" +if "%2"=="--Debug" set "buildMode=Debug" +if "%2"=="--Release" set "buildMode=Release" +if "%2"=="--RelWithDebInfo" set "buildMode=RelWithDebInfo" :noargs +set powershell=powershell +where powershell > nul 2>&1 +if ERRORLEVEL 1 goto :pwsh +echo found Powershell && goto start +:pwsh +set powershell=pwsh +where pwsh > nul 2>&1 +if ERRORLEVEL 1 goto :nopwsh +echo found pwsh && goto start +:nopwsh +echo Powershell or pwsh not found, please install it. +goto :eof + +:start chdir /d %ROOT_DIR% REM //---------- Check cmake version ---------- @@ -48,7 +64,7 @@ if ERRORLEVEL 1 ( REM //---------- get rpclib ---------- IF NOT EXIST external\rpclib mkdir external\rpclib IF NOT EXIST external\rpclib\rpclib-2.2.1 ( - REM //leave some blank lines because powershell shows download banner at top of console + REM //leave some blank lines because %powershell% shows download banner at top of console ECHO( ECHO( ECHO( @@ -56,13 +72,13 @@ IF NOT EXIST external\rpclib\rpclib-2.2.1 ( ECHO Downloading rpclib ECHO ***************************************************************************************** @echo on - powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip }" + %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip }" @echo off REM //remove any previous versions rmdir external\rpclib /q /s - powershell -command "& { Expand-Archive -Path external\rpclib.zip -DestinationPath external\rpclib }" + %powershell% -command "& { Expand-Archive -Path external\rpclib.zip -DestinationPath external\rpclib }" del external\rpclib.zip /q REM //Don't fail the build if the high-poly car is unable to be downloaded @@ -79,13 +95,11 @@ IF NOT EXIST external\rpclib\rpclib-2.2.1\build mkdir external\rpclib\rpclib-2.2 cd external\rpclib\rpclib-2.2.1\build cmake -G"Visual Studio 16 2019" .. -if "%buildMode%" == "--Debug" ( -cmake --build . --config Debug -) else if "%buildMode%" == "--Release" ( +if "%buildMode%" == "" ( +cmake --build . cmake --build . --config Release ) else ( -cmake --build . -cmake --build . --config Release +cmake --build . --config %buildMode% ) if ERRORLEVEL 1 goto :buildfailed @@ -98,20 +112,18 @@ set RPCLIB_TARGET_INCLUDE=AirLib\deps\rpclib\include if NOT exist %RPCLIB_TARGET_INCLUDE% mkdir %RPCLIB_TARGET_INCLUDE% robocopy /MIR external\rpclib\rpclib-2.2.1\include %RPCLIB_TARGET_INCLUDE% -if "%buildMode%" == "--Debug" ( +if "%buildMode%" == "" ( robocopy /MIR external\rpclib\rpclib-2.2.1\build\Debug %RPCLIB_TARGET_LIB%\Debug -) else if "%buildMode%" == "--Release" ( robocopy /MIR external\rpclib\rpclib-2.2.1\build\Release %RPCLIB_TARGET_LIB%\Release ) else ( -robocopy /MIR external\rpclib\rpclib-2.2.1\build\Debug %RPCLIB_TARGET_LIB%\Debug -robocopy /MIR external\rpclib\rpclib-2.2.1\build\Release %RPCLIB_TARGET_LIB%\Release +robocopy /MIR external\rpclib\rpclib-2.2.1\build\%buildMode% %RPCLIB_TARGET_LIB%\%buildMode% ) REM //---------- get High PolyCount SUV Car Model ------------ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv mkdir Unreal\Plugins\AirSim\Content\VehicleAdv IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 ( IF NOT DEFINED noFullPolyCar ( - REM //leave some blank lines because powershell shows download banner at top of console + REM //leave some blank lines because %powershell% shows download banner at top of console ECHO( ECHO( ECHO( @@ -123,12 +135,12 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 ( IF EXIST suv_download_tmp rmdir suv_download_tmp /q /s mkdir suv_download_tmp @echo on - REM powershell -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -Destination suv_download_tmp\car_assets.zip }" - REM powershell -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip', 'suv_download_tmp\car_assets.zip') }" - powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }" + REM %powershell% -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -Destination suv_download_tmp\car_assets.zip }" + REM %powershell% -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip', 'suv_download_tmp\car_assets.zip') }" + %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }" @echo off rmdir /S /Q Unreal\Plugins\AirSim\Content\VehicleAdv\SUV - powershell -command "& { Expand-Archive -Path suv_download_tmp\car_assets.zip -DestinationPath Unreal\Plugins\AirSim\Content\VehicleAdv }" + %powershell% -command "& { Expand-Archive -Path suv_download_tmp\car_assets.zip -DestinationPath Unreal\Plugins\AirSim\Content\VehicleAdv }" rmdir suv_download_tmp /q /s REM //Don't fail the build if the high-poly car is unable to be downloaded @@ -142,9 +154,9 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 ( REM //---------- get Eigen library ---------- IF NOT EXIST AirLib\deps mkdir AirLib\deps IF NOT EXIST AirLib\deps\eigen3 ( - powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip -OutFile eigen3.zip }" - powershell -command "& { Expand-Archive -Path eigen3.zip -DestinationPath AirLib\deps }" - powershell -command "& { Move-Item -Path AirLib\deps\eigen* -Destination AirLib\deps\del_eigen }" + %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip -OutFile eigen3.zip }" + %powershell% -command "& { Expand-Archive -Path eigen3.zip -DestinationPath AirLib\deps }" + %powershell% -command "& { Move-Item -Path AirLib\deps\eigen* -Destination AirLib\deps\del_eigen }" REM move AirLib\deps\eigen* AirLib\deps\del_eigen mkdir AirLib\deps\eigen3 move AirLib\deps\del_eigen\Eigen AirLib\deps\eigen3\Eigen @@ -155,16 +167,13 @@ IF NOT EXIST AirLib\deps\eigen3 goto :buildfailed REM //---------- now we have all dependencies to compile AirSim.sln which will also compile MavLinkCom ---------- -if "%buildMode%" == "--Debug" ( +if "%buildMode%" == "" ( msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Debug AirSim.sln if ERRORLEVEL 1 goto :buildfailed -) else if "%buildMode%" == "--Release" ( -msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Release AirSim.sln +msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Release AirSim.sln if ERRORLEVEL 1 goto :buildfailed ) else ( -msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Debug AirSim.sln -if ERRORLEVEL 1 goto :buildfailed -msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Release AirSim.sln +msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=%buildMode% AirSim.sln if ERRORLEVEL 1 goto :buildfailed ) diff --git a/docs/px4_logging.md b/docs/px4_logging.md index 9c91c1f55..996238030 100644 --- a/docs/px4_logging.md +++ b/docs/px4_logging.md @@ -4,7 +4,24 @@ Thanks to [Chris Lovett](https://github.com/clovett) for developing various tool ## Logging MavLink Messages -The following command will connect MavLinkTest app to the Simulator and enable logging +AirSim can capture mavlink log files if you add the following to the PX4 section of your `settings.json` file: + +```json +{ + "SettingsVersion": 1.2, + "SimMode": "Multirotor", + "Vehicles": { + "PX4": { + ..., + "Logs": "c:/temp/mavlink" + } + } +} +``` + +AirSim will create a timestamped log file in this folder for each "armed/disarmed" flight session. + +You can also create log files using the `MavLinkTest` app to the Simulator and enable logging of all mavlink commands to and from the PX4. ``` MavLinkTest -server:127.0.0.1:14550 -logdir:d:\temp diff --git a/docs/px4_sitl.md b/docs/px4_sitl.md index d0b242a20..7a117aee9 100644 --- a/docs/px4_sitl.md +++ b/docs/px4_sitl.md @@ -3,6 +3,9 @@ The [PX4](http://dev.px4.io) software provides a "software-in-loop" simulation (SITL) version of their stack that runs in Linux. If you are on Windows then you must use the [Cygwin Toolchain](https://dev.px4.io/master/en/setup/dev_env_windows_cygwin.html) as the [Bash On Windows](https://dev.px4.io/master/en/setup/dev_env_windows_bash_on_win.html) toolchain no longer works for SITL. +If you are using WSL2 please read these [additional +instructions](px4_sitl_wsl2.md). + **Note** that every time you stop the unreal app you have to restart the `px4` app. @@ -136,6 +139,11 @@ Local position: x=-0.0326988, y=0.00656854, z=5.48506 If the z coordinate is large like this then takeoff might not work as expected. Resetting the SITL and simulation should fix that problem. +## WSL 2 + +Windows Subsystem for Linux version 2 operates in a Virtual Machine. This requires +additional setup - see [additional instructions](px4_sitl_wsl2.md). + ## No Remote Control Notice the above setting is provided in the `params` section of the `settings.json` file: diff --git a/docs/px4_sitl_wsl2.md b/docs/px4_sitl_wsl2.md new file mode 100644 index 000000000..03baaa893 --- /dev/null +++ b/docs/px4_sitl_wsl2.md @@ -0,0 +1,106 @@ +# PX4 Software-in-Loop with WSL 2 + +The [Windows subsystem for Linux version +2](https://docs.microsoft.com/en-us/windows/wsl/install-win10) uses a Virtual Machine which has a +separate IP address from your Windows host machine. This means PX4 cannot find AirSim using +"localhost" which is the default behavior for PX4. + +You will notice that on Windows `ipconfig` returns a new ethernet adapter for WSL like this (notice +the vEthernet has `(WSL)` in the name: + +```plain +Ethernet adapter vEthernet (WSL): + + Connection-specific DNS Suffix . : + Link-local IPv6 Address . . . . . : fe80::1192:f9a5:df88:53ba%44 + IPv4 Address. . . . . . . . . . . : 172.31.64.1 + Subnet Mask . . . . . . . . . . . : 255.255.240.0 + Default Gateway . . . . . . . . . : +``` + +This address `172.31.64.1` is the address that WSL 2 can use to reach your Windows host machine. + +Starting with this [PX4 Change +Request](https://github.com/PX4/PX4-Autopilot/commit/1719ff9892f3c3d034f2b44e94d15527ab09cec6) PX4 +in SITL mode can now connect to AirSim on a different (remote) IP address. To enable this make sure +you have a version of PX4 containing this fix and set the following environment variable in linux: + +```shell +export PX4_SIM_HOST_ADDR=172.31.64.1 +``` + +**Note:** Be sure to update the above address `172.31.64.1` to match what you see from your +`ipconfig` command. + +Open incoming port 4560 using your Windows Firewall settings. + +Now on the linux side run `ip address show` and copy the `eth0 inet` address, it should be something +like `172.31.66.156`. This is the address Windows needs to know in order to find PX4. + +Edit your [AirSim settings](settings.md) file and add `LocalHostIp` to tell AirSim to use the WSL +ethernet adapter address instead of the default `localhost`. This will cause AirSim to open the TCP +port on that adapter which is the address that the PX4 app will be looking for. Also tell AirSim +to connect the `ControlIp` UDP channel by setting `ControlIp` to the magic string `remote`. +This resolves to the WSL 2 remote ip address found in the TCP socket. + +```json +{ + "SettingsVersion": 1.2, + "SimMode": "Multirotor", + "Vehicles": { + "PX4": { + "VehicleType": "PX4Multirotor", + "UseSerial": false, + "UseTcp": true, + "TcpPort": 4560, + "ControlIp": "remote", + "ControlPort": 14580, + "LocalHostIp": "172.31.64.1", + "Sensors":{ + "Barometer":{ + "SensorType": 1, + "Enabled": true, + "PressureFactorSigma": 0.0001825 + } + }, + "Parameters": { + "NAV_RCL_ACT": 0, + "NAV_DLL_ACT": 0, + "COM_OBL_ACT": 1, + "LPE_LAT": 47.641468, + "LPE_LON": -122.140165 + } + } + } +} +``` + +The "Barometer" setting keeps PX4 happy because the default AirSim barometer has a bit too much +noise generation. This setting clamps that down a bit. + +Lastly, please edit the Linux file in `ROMFS/px4fmu_common/init.d-posix/rcS` and make sure +it is looking for the `PX4_SIM_HOST_ADDR` environment variable and is passing that through to the +PX4 simulator like this: + +```shell +# If PX4_SIM_HOST_ADDR environment variable is empty use localhost. +if [ -z "${PX4_SIM_HOST_ADDR}" ]; then + echo "PX4 SIM HOST: localhost" + simulator start -c $simulator_tcp_port +else + echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR" + simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port +fi +``` + +**Note:** this code might already be there depending on the version of PX4 you are using. + +**Note:** please be patient when waiting for the message: + +``` +INFO [simulator] Simulator connected on TCP port 4560. +``` + +It can take a little longer to establish the remote connection than it does with `localhost`. + +Now you can proceed with the steps shown in [Setting up PX4 Software-in-Loop](px4_sitl.md). diff --git a/mkdocs.yml b/mkdocs.yml index aa373a200..e47e8e57b 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -85,6 +85,7 @@ nav: - "MavLink and PX4": - "PX4 Setup for AirSim": 'px4_setup.md' - "PX4 in SITL": 'px4_sitl.md' + - "PX4 SITL with WSL 2": 'px4_sitl_wsl2.md' - "PX4 Multi-vehicle in SITL": 'px4_multi_vehicle.md' - "AirSim with Pixhawk": 'https://youtu.be/1oY8Qu5maQQ' - "PX4 Setup with AirSim": 'https://youtu.be/HNWdYrtw3f0'