diff --git a/.gitignore b/.gitignore index 4026d1e5f2..928e34e2ad 100644 --- a/.gitignore +++ b/.gitignore @@ -320,4 +320,6 @@ deps/ /PythonClient/cloud.txt /Unreal/Plugins/AirSim/Content/VehicleAdv/SUV/ /suv_download_tmp -car_assets.zip \ No newline at end of file +car_assets.zip +/PythonClient/cloud.txt +__pycache__/ \ No newline at end of file diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index c1033bf527..fe79895790 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -78,7 +78,7 @@ class FastPhysicsEngine : public PhysicsEngineBase { //first compute the response as if there was no collision //this is necessory to take in to account forces and torques generated by body - getNextKinematicsNoCollision(dt, body, current, next, next_wrench); + getNextKinematicsNoCollision(dt, body, current, next, next_wrench, grounded_); //if there is collision, see if we need collision response const CollisionInfo collision_info = body.getCollisionInfo(); @@ -86,9 +86,11 @@ class FastPhysicsEngine : public PhysicsEngineBase { //if collision was already responsed then do not respond to it until we get updated information if (collision_info.has_collided && collision_response_info.collision_time_stamp != collision_info.time_stamp) { bool is_collision_response = getNextKinematicsOnCollision(dt, collision_info, body, - current, next, next_wrench, enable_ground_lock_); + current, next, next_wrench, enable_ground_lock_, grounded_); updateCollisionResponseInfo(collision_info, next, is_collision_response, collision_response_info); + //throttledLogOutput("*** has collision", 0.1); } + //else throttledLogOutput("*** no collision", 0.1); //Utils::log(Utils::stringf("T-VEL %s %" PRIu64 ": ", // VectorMath::toString(next.twist.linear).c_str(), clock()->getStepCount())); @@ -98,7 +100,8 @@ class FastPhysicsEngine : public PhysicsEngineBase { body.kinematicsUpdated(); } - static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next, bool is_collision_response, CollisionResponseInfo& collision_response_info) + static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next, + bool is_collision_response, CollisionResponseInfo& collision_response_info) { collision_response_info.collision_time_stamp = collision_info.time_stamp; ++collision_response_info.collision_count_raw; @@ -111,7 +114,7 @@ class FastPhysicsEngine : public PhysicsEngineBase { //return value indicates if collision response was generated static bool getNextKinematicsOnCollision(TTimeDelta dt, const CollisionInfo& collision_info, const PhysicsBody& body, - const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench, bool enable_ground_lock) + const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench, bool enable_ground_lock, bool& grounded) { /************************* Collision response ************************/ const real_T dt_real = static_cast(dt); @@ -133,7 +136,7 @@ class FastPhysicsEngine : public PhysicsEngineBase { //see if impact is straight at body's surface (assuming its box) const Vector3r normal_body = VectorMath::transformToBodyFrame(collision_info.normal, current.pose.orientation); const bool is_ground_normal = Utils::isApproximatelyEqual(std::abs(normal_body.z()), 1.0f, kAxisTolerance); - bool ground_lock = false; + bool ground_collision = false; if (is_ground_normal || Utils::isApproximatelyEqual(std::abs(normal_body.x()), 1.0f, kAxisTolerance) || Utils::isApproximatelyEqual(std::abs(normal_body.y()), 1.0f, kAxisTolerance) @@ -142,12 +145,12 @@ class FastPhysicsEngine : public PhysicsEngineBase { r = Vector3r::Zero(); //we have collided with ground straight on, we will fix orientation later - ground_lock = is_ground_normal; + ground_collision = is_ground_normal; } //velocity at contact point const Vector3r vcur_avg_body = VectorMath::transformToBodyFrame(vcur_avg, current.pose.orientation); - const Vector3r contact_vel_body = vcur_avg_body + angular_avg.cross(r); + const Vector3r contact_vel_body = vcur_avg_body + angular_avg.cross(r); /* GafferOnGames - Collision response with columb friction @@ -189,7 +192,7 @@ class FastPhysicsEngine : public PhysicsEngineBase { next.accelerations.angular = Vector3r::Zero(); next.pose = current.pose; - if (enable_ground_lock && ground_lock) { + if (enable_ground_lock && ground_collision) { float pitch, roll, yaw; VectorMath::toEulerianAngle(next.pose.orientation, pitch, roll, yaw); pitch = roll = 0; @@ -197,10 +200,18 @@ class FastPhysicsEngine : public PhysicsEngineBase { //there is a lot of random angular velocity when vehicle is on the ground next.twist.angular = Vector3r::Zero(); - } - //else keep the orientation - next.pose.position = collision_info.position + (collision_info.normal * collision_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles); + // also eliminate any linear velocity due to twist - since we are sitting on the ground there shouldn't be any. + next.twist.linear = Vector3r::Zero(); + next.pose.position = collision_info.position; + grounded = true; + //throttledLogOutput("*** Triggering ground lock", 0.1); + } + else + { + //else keep the orientation + next.pose.position = collision_info.position + (collision_info.normal * collision_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles); + } next_wrench = Wrench::zero(); //Utils::log(Utils::stringf("*** C-VEL %s: ", VectorMath::toString(next.twist.linear).c_str())); @@ -208,6 +219,17 @@ class FastPhysicsEngine : public PhysicsEngineBase { return true; } + void throttledLogOutput(const std::string& msg, double seconds) + { + TTimeDelta dt = clock()->elapsedSince(last_message_time); + const real_T dt_real = static_cast(dt); + if (dt_real > seconds) + { + Utils::log(msg); + last_message_time = clock()->nowNanos(); + } + } + //bool getNextKinematicsOnGround(TTimeDelta dt, const PhysicsBody& body, const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench) //{ // /************************* reset state if we have hit the ground ************************/ @@ -292,59 +314,84 @@ class FastPhysicsEngine : public PhysicsEngineBase { return wrench; } - static void getNextKinematicsNoCollision(TTimeDelta dt, const PhysicsBody& body, const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench) + static void getNextKinematicsNoCollision(TTimeDelta dt, const PhysicsBody& body, const Kinematics::State& current, + Kinematics::State& next, Wrench& next_wrench, bool& grounded) { const real_T dt_real = static_cast(dt); + Vector3r avg_linear = Vector3r::Zero(); + Vector3r avg_angular = Vector3r::Zero(); + /************************* Get force and torque acting on body ************************/ //set wrench sum to zero const Wrench body_wrench = getBodyWrench(body, current.pose.orientation); - //add linear drag due to velocity we had since last dt seconds - //drag vector magnitude is proportional to v^2, direction opposite of velocity - //total drag is b*v + c*v*v but we ignore the first term as b << c (pg 44, Classical Mechanics, John Taylor) - //To find the drag force, we find the magnitude in the body frame and unit vector direction in world frame - const Vector3r avg_linear = current.twist.linear + current.accelerations.linear * (0.5f * dt_real); - const Vector3r avg_angular = current.twist.angular + current.accelerations.angular * (0.5f * dt_real); - const Wrench drag_wrench = getDragWrench(body, current.pose.orientation, avg_linear, avg_angular); - - next_wrench = body_wrench + drag_wrench; - - //Utils::log(Utils::stringf("B-WRN %s: ", VectorMath::toString(body_wrench.force).c_str())); - //Utils::log(Utils::stringf("D-WRN %s: ", VectorMath::toString(drag_wrench.force).c_str())); - - /************************* Update accelerations due to force and torque ************************/ - //get new acceleration due to force - we'll use this acceleration in next time step - next.accelerations.linear = (next_wrench.force / body.getMass()) + body.getEnvironment().getState().gravity; + if (grounded) { + // make it stick to the ground until we see significant body wrench forces. + float normalizedForce = body_wrench.force.squaredNorm(); + if (normalizedForce > kSurfaceTension) + { + //throttledLogOutput("*** Losing ground lock due to body_wrench " + VectorMath::toString(body_wrench.force), 0.1); + grounded = false; + } + next_wrench.force = Vector3r::Zero(); + next_wrench.torque = Vector3r::Zero(); + next.accelerations.linear = Vector3r::Zero(); + } + else { + //add linear drag due to velocity we had since last dt seconds + //drag vector magnitude is proportional to v^2, direction opposite of velocity + //total drag is b*v + c*v*v but we ignore the first term as b << c (pg 44, Classical Mechanics, John Taylor) + //To find the drag force, we find the magnitude in the body frame and unit vector direction in world frame + avg_linear = current.twist.linear + current.accelerations.linear * (0.5f * dt_real); + avg_angular = current.twist.angular + current.accelerations.angular * (0.5f * dt_real); + const Wrench drag_wrench = getDragWrench(body, current.pose.orientation, avg_linear, avg_angular); - //get new angular acceleration - //Euler's rotation equation: https://en.wikipedia.org/wiki/Euler's_equations_(body_dynamics) - //we will use torque to find out the angular acceleration - //angular momentum L = I * omega - const Vector3r angular_momentum = body.getInertia() * avg_angular; - const Vector3r angular_momentum_rate = next_wrench.torque - avg_angular.cross(angular_momentum); - //new angular acceleration - we'll use this acceleration in next time step - next.accelerations.angular = body.getInertiaInv() * angular_momentum_rate; + next_wrench = body_wrench + drag_wrench; + //Utils::log(Utils::stringf("B-WRN %s: ", VectorMath::toString(body_wrench.force).c_str())); + //Utils::log(Utils::stringf("D-WRN %s: ", VectorMath::toString(drag_wrench.force).c_str())); + /************************* Update accelerations due to force and torque ************************/ + //get new acceleration due to force - we'll use this acceleration in next time step - /************************* Update pose and twist after dt ************************/ - //Verlet integration: http://www.physics.udel.edu/~bnikolic/teaching/phys660/numerical_ode/node5.html - next.twist.linear = current.twist.linear + (current.accelerations.linear + next.accelerations.linear) * (0.5f * dt_real); - next.twist.angular = current.twist.angular + (current.accelerations.angular + next.accelerations.angular) * (0.5f * dt_real); + next.accelerations.linear = (next_wrench.force / body.getMass()) + body.getEnvironment().getState().gravity; + } - //if controller has bug, velocities can increase idenfinitely - //so we need to clip this or everything will turn in to infinity/nans - if (next.twist.linear.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light - next.twist.linear /= (next.twist.linear.norm() / EarthUtils::SpeedOfLight); - next.accelerations.linear = Vector3r::Zero(); - } - // - //for disc of 1m radius which angular velocity translates to speed of light on tangent? - if (next.twist.angular.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light - next.twist.angular /= (next.twist.angular.norm() / EarthUtils::SpeedOfLight); + if (grounded) { + // this stops vehicle from vibrating while it is on the ground doing nothing. next.accelerations.angular = Vector3r::Zero(); + next.twist.linear = Vector3r::Zero(); + next.twist.angular = Vector3r::Zero(); + } else { + //get new angular acceleration + //Euler's rotation equation: https://en.wikipedia.org/wiki/Euler's_equations_(body_dynamics) + //we will use torque to find out the angular acceleration + //angular momentum L = I * omega + const Vector3r angular_momentum = body.getInertia() * avg_angular; + const Vector3r angular_momentum_rate = next_wrench.torque - avg_angular.cross(angular_momentum); + //new angular acceleration - we'll use this acceleration in next time step + next.accelerations.angular = body.getInertiaInv() * angular_momentum_rate; + + /************************* Update pose and twist after dt ************************/ + //Verlet integration: http://www.physics.udel.edu/~bnikolic/teaching/phys660/numerical_ode/node5.html + next.twist.linear = current.twist.linear + (current.accelerations.linear + next.accelerations.linear) * (0.5f * dt_real); + next.twist.angular = current.twist.angular + (current.accelerations.angular + next.accelerations.angular) * (0.5f * dt_real); + + //if controller has bug, velocities can increase idenfinitely + //so we need to clip this or everything will turn in to infinity/nans + + if (next.twist.linear.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light + next.twist.linear /= (next.twist.linear.norm() / EarthUtils::SpeedOfLight); + next.accelerations.linear = Vector3r::Zero(); + } + // + //for disc of 1m radius which angular velocity translates to speed of light on tangent? + if (next.twist.angular.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light + next.twist.angular /= (next.twist.angular.norm() / EarthUtils::SpeedOfLight); + next.accelerations.angular = Vector3r::Zero(); + } } computeNextPose(dt, current.pose, avg_linear, avg_angular, next); @@ -399,11 +446,12 @@ class FastPhysicsEngine : public PhysicsEngineBase { static constexpr float kAxisTolerance = 0.25f; static constexpr float kRestingVelocityMax = 0.1f; static constexpr float kDragMinVelocity = 0.1f; + static constexpr float kSurfaceTension = 1.0f; std::stringstream debug_string_; - int grounded_; + bool grounded_ = false; bool enable_ground_lock_; - + TTimePoint last_message_time; }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp b/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp index ea4562c6b7..7f75f215c3 100644 --- a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp @@ -49,7 +49,7 @@ class MavLinkDroneController : public DroneControllerBase /* Default values are requires so uninitialized instance doesn't have random values */ bool use_serial = true; // false means use UDP instead - //Used to connect via HITL: needed only if use_serial = true + //Used to connect via HITL: needed only if use_serial = true std::string serial_port = "*"; int baud_rate = 115200; @@ -77,7 +77,7 @@ class MavLinkDroneController : public DroneControllerBase uint8_t offboard_sysid = 134; int offboard_compid = 1; uint8_t vehicle_sysid = 135; - int vehicle_compid = 1; + int vehicle_compid = 1; // if you want to select a specific local network adapter so you can reach certain remote machines (e.g. wifi versus ethernet) // then you will want to change the LocalHostIp accordingly. This default only works when log viewer and QGC are also on the @@ -128,7 +128,7 @@ class MavLinkDroneController : public DroneControllerBase bool armDisarm(bool arm, CancelableBase& cancelable_action) override; bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action) override; bool land(float max_wait_seconds, CancelableBase& cancelable_action) override; - bool goHome(CancelableBase& cancelable_action) override; + bool goHome(CancelableBase& cancelable_action) override; bool hover(CancelableBase& cancelable_action) override; GeoPoint getHomeGeoPoint() override; GeoPoint getGpsLocation() override; @@ -140,7 +140,7 @@ class MavLinkDroneController : public DroneControllerBase virtual bool loopCommandPre() override; virtual void loopCommandPost() override; -protected: +protected: void commandRollPitchZ(float pitch, float roll, float z, float yaw) override; void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override; void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override; @@ -259,7 +259,7 @@ struct MavLinkDroneController::impl { message = is_available_message_; return is_available_; } - + ConnectionInfo getMavConnectionInfo() { return connection_info_; @@ -293,7 +293,7 @@ struct MavLinkDroneController::impl { is_controls_0_1_ = true; Utils::setValue(rotor_controls_, 0.0f); //TODO: main_node_->setMessageInterval(...); - connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg){ + connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { unused(connection); processMavMessages(msg); }); @@ -366,7 +366,7 @@ struct MavLinkDroneController::impl { qgc_proxy_ = nullptr; } else { - connection->subscribe([=](std::shared_ptr connection_val, const mavlinkcom::MavLinkMessage& msg){ + connection->subscribe([=](std::shared_ptr connection_val, const mavlinkcom::MavLinkMessage& msg) { unused(connection_val); processQgcMessages(msg); }); @@ -406,13 +406,13 @@ struct MavLinkDroneController::impl { { mavlinkcom::SerialPortInfo info = *iter; if ( - ( - (info.vid == pixhawkVendorId) && - (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId) + ( + (info.vid == pixhawkVendorId) && + (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId) ) || ( - (info.displayName.find(L"PX4_") != std::string::npos) - ) + (info.displayName.find(L"PX4_") != std::string::npos) + ) ) { // printf("Auto Selecting COM port: %S\n", info.displayName.c_str()); @@ -423,7 +423,7 @@ struct MavLinkDroneController::impl { } void connect() - { + { createMavConnection(connection_info_); initializeMavSubscriptions(); } @@ -454,7 +454,7 @@ struct MavLinkDroneController::impl { addStatusMessage(Utils::stringf("Connecting to UDP port %d, local IP %s, remote IP...", port, connection_info_.local_host_ip.c_str(), ip.c_str())); connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, ip, port); - hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); + hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); hil_node_->connect(connection_); addStatusMessage(std::string("Connected over UDP.")); @@ -463,11 +463,11 @@ struct MavLinkDroneController::impl { if (connection_info_.sitl_ip_address != "" && connection_info_.sitl_ip_port != 0 && connection_info_.sitl_ip_port != port) { // bugbug: the PX4 SITL mode app cannot receive commands to control the drone over the same mavlink connection // as the HIL_SENSOR messages, we must establish a separate mavlink channel for that so that DroneShell works. - addStatusMessage(Utils::stringf("Connecting to PX4 SITL UDP port %d, local IP %s, remote IP...", + addStatusMessage(Utils::stringf("Connecting to PX4 SITL UDP port %d, local IP %s, remote IP...", connection_info_.sitl_ip_port, connection_info_.local_host_ip.c_str(), connection_info_.sitl_ip_address.c_str())); - auto sitlconnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("sitl", - connection_info_.local_host_ip, connection_info_.sitl_ip_address, connection_info_.sitl_ip_port); + auto sitlconnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("sitl", + connection_info_.local_host_ip, connection_info_.sitl_ip_address, connection_info_.sitl_ip_port); mav_vehicle_->connect(sitlconnection); addStatusMessage(std::string("Connected to SITL over UDP.")); @@ -625,10 +625,10 @@ struct MavLinkDroneController::impl { mavlinkcom::MavLinkHilSensor hil_sensor; hil_sensor.time_usec = static_cast(Utils::getTimeSinceEpochNanos() / 1000.0); + hil_sensor.xacc = acceleration.x(); hil_sensor.yacc = acceleration.y(); hil_sensor.zacc = acceleration.z(); - hil_sensor.xgyro = gyro.x(); hil_sensor.ygyro = gyro.y(); hil_sensor.zgyro = gyro.z(); @@ -639,7 +639,7 @@ struct MavLinkDroneController::impl { hil_sensor.abs_pressure = abs_pressure; hil_sensor.pressure_alt = pressure_alt; - //TODO: enable tempeprature? diff_presure + //TODO: enable temperature? diff_pressure hil_sensor.fields_updated = was_reset_ ? (1 << 31) : 0; if (hil_node_ != nullptr) { @@ -746,8 +746,8 @@ struct MavLinkDroneController::impl { const auto& mag_output = getMagnetometer()->getOutput(); const auto& baro_output = getBarometer()->getOutput(); - sendHILSensor(imu_output.linear_acceleration, - imu_output.angular_velocity, + sendHILSensor(imu_output.linear_acceleration, + imu_output.angular_velocity, mag_output.magnetic_field_body, baro_output.pressure * 0.01f /*Pa to Milibar */, baro_output.altitude); @@ -808,8 +808,8 @@ struct MavLinkDroneController::impl { void getMocapPose(Vector3r& position, Quaternionr& orientation) { - position.x() = MocapPoseMessage.x; position.y() = MocapPoseMessage.y; position.z() = MocapPoseMessage.z; - orientation.w() = MocapPoseMessage.q[0]; orientation.x() = MocapPoseMessage.q[1]; + position.x() = MocapPoseMessage.x; position.y() = MocapPoseMessage.y; position.z() = MocapPoseMessage.z; + orientation.w() = MocapPoseMessage.q[0]; orientation.x() = MocapPoseMessage.q[1]; orientation.y() = MocapPoseMessage.q[2]; orientation.z() = MocapPoseMessage.q[3]; } @@ -1035,7 +1035,7 @@ struct MavLinkDroneController::impl { if (max_wait_seconds <= 0) return true; // client doesn't want to wait. - return parent_->waitForZ(max_wait_seconds, z, getDistanceAccuracy(), cancelable_action); + return parent_->waitForZ(max_wait_seconds, z, getDistanceAccuracy(), cancelable_action); } bool hover(CancelableBase& cancelable_action) @@ -1068,11 +1068,11 @@ struct MavLinkDroneController::impl { { throw VehicleMoveException("Landing command - timeout waiting for response from drone"); } - else if(!rc) { + else if (!rc) { throw VehicleMoveException("Landing command rejected by drone"); } } - else + else { throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude. Could fix this if we hook up a distance to ground sensor..."); } @@ -1150,17 +1150,17 @@ struct MavLinkDroneController::impl { } //drone parameters - float getCommandPeriod() + float getCommandPeriod() { - return 1.0f/50; //1 period of 50hz + return 1.0f / 50; //1 period of 50hz } float getTakeoffZ() { // pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe // enough to get out of the backwash turbulance. Negative due to NED coordinate system. - return -3.0f; + return -3.0f; } - float getDistanceAccuracy() + float getDistanceAccuracy() { return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance travelled } @@ -1250,7 +1250,7 @@ struct MavLinkDroneController::impl { ensureSafeMode(); } - void ensureSafeMode() + void ensureSafeMode() { if (mav_vehicle_ != nullptr) { const mavlinkcom::VehicleState& state = mav_vehicle_->getVehicleState(); @@ -1456,7 +1456,7 @@ void MavLinkDroneController::loopCommandPost() } //drone parameters -float MavLinkDroneController::getCommandPeriod() +float MavLinkDroneController::getCommandPeriod() { return pimpl_->getCommandPeriod(); } @@ -1464,7 +1464,7 @@ float MavLinkDroneController::getTakeoffZ() { return pimpl_->getTakeoffZ(); } -float MavLinkDroneController::getDistanceAccuracy() +float MavLinkDroneController::getDistanceAccuracy() { return pimpl_->getDistanceAccuracy(); } diff --git a/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs b/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs index abad34c1c4..b6932c5d4f 100644 --- a/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs +++ b/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs @@ -244,6 +244,8 @@ public void SetData(DataSeries series) { this.dirty = true; this.series = series; + this.currentValue = null; + this.scaleTransform = null; this.UpdateLayout(); if (this.ActualWidth != 0) @@ -477,13 +479,33 @@ void UpdateChart() if (liveScrolling) { // just show the tail that fits on screen, since the scaling will not happen on x-axis in this case. - this.visibleStartIndex = series.Values.Count - (int)this.ActualWidth; + var width = this.ActualWidth; + this.visibleEndIndex = series.Values.Count; + this.visibleStartIndex = this.visibleEndIndex; + + if (series.Values.Count > 0) + { + // walk back until the scaled values fill one screen width. + this.visibleStartIndex = this.visibleEndIndex - 1; + Point endPoint = GetScaledValue(series.Values[this.visibleStartIndex]); + while (--this.visibleStartIndex > 0) + { + Point p = GetScaledValue(series.Values[this.visibleStartIndex]); + if (endPoint.X - p.X > width) + { + break; + } + } + } + minY = double.MaxValue; maxY = double.MinValue; minX = double.MaxValue; maxX = double.MinValue; this.smoothScrollIndex = this.series.Values.Count; + + ComputeScale(); } double count = series.Values.Count; @@ -527,7 +549,7 @@ private void AddScaledValues(PathFigure figure, int start, int end) double rx = pt.X + offset; - if (pt.X >= 0 && pt.X < width) + if (pt.X >= 0) { visibleCount++; if (!started) diff --git a/LogViewer/LogViewer/LogViewer.csproj b/LogViewer/LogViewer/LogViewer.csproj index 2b7869eee0..2b307707de 100644 --- a/LogViewer/LogViewer/LogViewer.csproj +++ b/LogViewer/LogViewer/LogViewer.csproj @@ -32,7 +32,7 @@ PX4 Log Viewer true publish.htm - 41 + 43 1.0.0.%2a false true diff --git a/LogViewer/LogViewer/MainWindow.xaml.cs b/LogViewer/LogViewer/MainWindow.xaml.cs index 6379b9dfa8..3a09a0b787 100644 --- a/LogViewer/LogViewer/MainWindow.xaml.cs +++ b/LogViewer/LogViewer/MainWindow.xaml.cs @@ -38,6 +38,7 @@ public partial class MainWindow : Window Quaternion initialAttitude; MapPolyline currentFlight; MavlinkLog currentFlightLog; + long lastAttitudeMessage; public MainWindow() { @@ -146,17 +147,22 @@ private void OnMavlinkMessageReceived(object sender, MavLinkMessage e) { case MAVLink.MAVLINK_MSG_ID.ATTITUDE_QUATERNION: { - var payload = (MAVLink.mavlink_attitude_quaternion_t)e.TypedPayload; - var q = new System.Windows.Media.Media3D.Quaternion(payload.q1, payload.q2, payload.q3, payload.q4); - UiDispatcher.RunOnUIThread(() => - { - //ModelViewer.ModelAttitude = initialAttitude * q; + // Only do this if drone is not sending MAVLINK_MSG_ID.ATTITUDE... + if (Environment.TickCount - lastAttitudeMessage > 1000) + { + var payload = (MAVLink.mavlink_attitude_quaternion_t)e.TypedPayload; + var q = new System.Windows.Media.Media3D.Quaternion(payload.q1, payload.q2, payload.q3, payload.q4); + UiDispatcher.RunOnUIThread(() => + { + ModelViewer.ModelAttitude = initialAttitude * q; }); + } break; } case MAVLink.MAVLINK_MSG_ID.ATTITUDE: { + lastAttitudeMessage = Environment.TickCount; var payload = (MAVLink.mavlink_attitude_t)e.TypedPayload; Quaternion y = new Quaternion(new Vector3D(0, 0, 1), -payload.yaw * 180 / Math.PI); Quaternion x = new Quaternion(new Vector3D(1, 0, 0), payload.pitch * 180 / Math.PI); @@ -999,11 +1005,13 @@ private void GraphItem(LogItemSchema schema) List values = new List(currentFlightLog.GetDataValues(schema, DateTime.MinValue, TimeSpan.MaxValue)); chart = AddChart(schema, values); - // now turn on live scrolling... - chart.LiveScrolling = true; - // the X values are in microseconds (s0 the numerator is the speed of scrolling). - chart.LiveScrollingXScale = 50.0 / 1000000.0; - + if (!pauseRecording) + { + // now turn on live scrolling if we are recording... + chart.LiveScrolling = true; + // the X values are in microseconds (s0 the numerator is the speed of scrolling). + chart.LiveScrollingXScale = 50.0 / 1000000.0; + } liveScrolling.Add(chart); // now start watching the live update for new values that need to be added to this chart. diff --git a/MavLinkCom/MavLinkTest/main.cpp b/MavLinkCom/MavLinkTest/main.cpp index cb56e4201f..1bf3cc17c5 100644 --- a/MavLinkCom/MavLinkTest/main.cpp +++ b/MavLinkCom/MavLinkTest/main.cpp @@ -1222,7 +1222,7 @@ void handleStatus(const MavLinkStatustext& statustext) { } std::string safeText(statustext.text, 50); - Utils::log(Utils::stringf("STATUS: sev=%d, '%s'", static_cast(statustext.severity), safeText.c_str())); + Utils::log(Utils::stringf("STATUS: sev=%d, '%s'\n", static_cast(statustext.severity), safeText.c_str())); } int console(std::stringstream& script) { diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index dfc7237464..6ccb09cd2e 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -70,6 +70,7 @@ Code + 10.0 diff --git a/PythonClient/path.py b/PythonClient/path.py index e834ab0909..29050bb629 100644 --- a/PythonClient/path.py +++ b/PythonClient/path.py @@ -5,17 +5,20 @@ client = MultirotorClient() client.confirmConnection() client.enableApiControl(True) + client.armDisarm(True) client.takeoff() # AirSim uses NED coordinates so negative axis is up. -# z of -5 is 5 meters above the original launch point. -z = -5 +# z of -7 is 7 meters above the original launch point. +z = -7 # see https://github.com/Microsoft/AirSim/wiki/moveOnPath-demo # this method is async and we are not waiting for the result since we are passing max_wait_seconds=0. -print("client.moveOnPath to fly fast path along the streets") -result = client.moveOnPath([Vector3r(0,-253,z),Vector3r(125,-253,z),Vector3r(125,0,z),Vector3r(0,0,z)], - 15, 60, - DrivetrainType.ForwardOnly, YawMode(False,0), 20, 1) +result = client.moveOnPath([Vector3r(0,-253,z),Vector3r(125,-253,z),Vector3r(125,0,z),Vector3r(0,0,z),Vector3r(0,0,-20)], + 15, 65, + DrivetrainType.ForwardOnly, YawMode(False,0), 20, 1) +client.land() +client.armDisarm(False) +client.enableApiControl(False) diff --git a/PythonClient/survey.py b/PythonClient/survey.py new file mode 100644 index 0000000000..094b056e69 --- /dev/null +++ b/PythonClient/survey.py @@ -0,0 +1,89 @@ +from AirSimClient import * +import sys +import time +import argparse + +class SurveyNavigator: + def __init__(self, args): + self.boxsize = args.size + self.stripewidth = args.stripewidth + self.altitude = args.altitude + self.velocity = args.speed + self.client = MultirotorClient() + self.client.confirmConnection() + self.client.enableApiControl(True) + + def start(self): + print("arming the drone...") + self.client.armDisarm(True) + + landed = self.client.getLandedState() + if landed != 1: + print("taking off...") + self.client.takeoff() + + # AirSim uses NED coordinates so negative axis is up. + x = -self.boxsize + z = -self.altitude + + print("climing to altitude: " + str(self.altitude)) + self.client.moveToPosition(0, 0, z, self.velocity) + + print("flying to first corner of survey box") + self.client.moveToPosition(x, -self.boxsize, z, self.velocity) + + # let it settle there a bit. + self.client.hover() + time.sleep(2) + + # now compute the survey path required to fill the box + path = [] + distance = 0 + while x < self.boxsize: + distance += self.boxsize + path.append(Vector3r(x, self.boxsize, z)) + x += self.stripewidth + distance += self.stripewidth + path.append(Vector3r(x, self.boxsize, z)) + distance += self.boxsize + path.append(Vector3r(x, -self.boxsize, z)) + x += self.stripewidth + distance += self.stripewidth + path.append(Vector3r(x, -self.boxsize, z)) + distance += self.boxsize + + print("starting survey, estimated distance is " + str(distance)) + trip_time = distance / self.velocity + print("estimated survey time is " + str(trip_time)) + try: + self.client.enableApiControl(True) + result = self.client.moveOnPath(path, self.velocity, trip_time, DrivetrainType.ForwardOnly, YawMode(False,0), self.velocity + (self.velocity/2), 1) + except: + errorType, value, traceback = sys.exc_info() + print("moveOnPath threw exception: " + str(value)) + pass + + print("flying back home") + self.client.moveToPosition(0, 0, z, self.velocity) + + if z < -5: + print("descending") + self.client.moveToPosition(0, 0, -5, 2) + + print("landing...") + self.client.land() + + print("disarming.") + self.client.armDisarm(False) + +if __name__ == "__main__": + args = sys.argv + args.pop(0) + arg_parser = argparse.ArgumentParser("Usage: survey boxsize stripewidth altitude") + arg_parser.add_argument("--size", type=float, help="size of the box to survey") + arg_parser.add_argument("--stripewidth", type=float, help="stripe width of survey (in meters)") + arg_parser.add_argument("--altitude", type=float, help="altitude of survey (in positive meters)") + arg_parser.add_argument("--speed", type=float, help="speed of survey (in meters/second)") + args = arg_parser.parse_args(args) + nav = SurveyNavigator(args) + nav.start() diff --git a/docs/px4_sitl.md b/docs/px4_sitl.md index c7aba859f4..ec82f1bca1 100644 --- a/docs/px4_sitl.md +++ b/docs/px4_sitl.md @@ -16,7 +16,7 @@ you can build and run it there. ``` 3. Use following command to start PX4 firmware in SITL mode: ``` - ./build_posix_sitl_default/src/firmware/posix/px4 ./posix-configs/SITL/init/ekf2/iris + ./build/posix_sitl_default/px4 ./posix-configs/SITL/init/ekf2/iris ``` 4. You should see a message like this you `INFO [simulator] Waiting for initial data on UDP port 14560` which means the SITL PX4 app is waiting for someone to connect. 5. Now edit [AirSim settings](settings.md) file to make sure you have followings: @@ -28,7 +28,7 @@ you can build and run it there. } } ``` -6. Run Unreal environment and it should connect to SITL via UDP. You should see a bunch of messages from the SITL PX4 window from things like `local_position_estimator` and `commander` and so on. +6. Run Unreal environment and it should connect to SITL via UDP. You should see a bunch of messages from the SITL PX4 window from things like `[mavlink]` and `[commander]` and so on. 7. You should also be able to use QGroundControl just like with flight controller hardware. Note that as we don't have physical board, RC cannot be connected directly to it. So the alternatives are either use XBox 360 Controller or connect your RC using USB (for example, in case of FrSky Taranis X9D Plus) or using trainer USB cable to PC. This makes your RC look like joystick. You will need to do extra set up in QGroundControl to use virtual joystick for RC control. ## Setting GPS origin