AirSim+PX4+QGC实现无人机自动避障
博客地址：https://www.cnblogs.com/zylyehuo/参考链接： AirSim+PX4 无人机自动避障详细教程
AirSim 配置文件修改
代码修改完成后，进入 AirSim 根目录运行如下代码进行编译
./setup.sh
./build.sh运行完成后，将 Plugins 文件夹重新复制到 UE4 根目录下
MavLinkConnectionImpl.cpp
// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License.
#include "MavLinkMessages.hpp"
#include "MavLinkConnectionImpl.hpp"
#include "Utils.hpp"
#include "ThreadUtils.hpp"
#include "../serial_com/Port.h"
#include "../serial_com/SerialPort.hpp"
#include "../serial_com/UdpClientPort.hpp"
#include "../serial_com/TcpClientPort.hpp"
using namespace mavlink_utils;
using namespace mavlinkcom_impl;
// hw-modify 全局字符串变量，用来在MavLinkMultirotorApi.hpp的update函数中打印调试信息。后续该改成正常打印的方式。
std::string hw_extern_msg = Utils::stringf("---hw---: Global print string");
MavLinkConnectionImpl::MavLinkConnectionImpl()
{
// add our custom telemetry message length.
telemetry_.crc_errors = 0;
telemetry_.handler_microseconds = 0;
telemetry_.messages_handled = 0;
telemetry_.messages_received = 0;
telemetry_.messages_sent = 0;
closed = true;
::memset(&mavlink_intermediate_status_, 0, sizeof(mavlink_status_t));
::memset(&mavlink_status_, 0, sizeof(mavlink_status_t));
// todo: if we support signing then initialize
// mavlink_intermediate_status_.signing callbacks
}
std::string MavLinkConnectionImpl::getName()
{
return name;
}
MavLinkConnectionImpl::~MavLinkConnectionImpl()
{
con_.reset();
close();
}
std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::createConnection(const std::string& nodeName, std::shared_ptr<Port> port)
{
// std::shared_ptr<MavLinkCom> owner, const std::string& nodeName
std::shared_ptr<MavLinkConnection> con = std::make_shared<MavLinkConnection>();
con->startListening(nodeName, port);
return con;
}
std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort)
{
std::shared_ptr<UdpClientPort> socket = std::make_shared<UdpClientPort>();
socket->connect(localAddr, localPort, "", 0);
return createConnection(nodeName, socket);
}
std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort)
{
std::string local = localAddr;
// just a little sanity check on the local address, if remoteAddr is localhost then localAddr must be also.
if (remoteAddr == "127.0.0.1") {
local = "127.0.0.1";
}
std::shared_ptr<UdpClientPort> socket = std::make_shared<UdpClientPort>();
socket->connect(local, 0, remoteAddr, remotePort);
return createConnection(nodeName, socket);
}
std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort)
{
std::string local = localAddr;
// just a little sanity check on the local address, if remoteAddr is localhost then localAddr must be also.
if (remoteIpAddr == "127.0.0.1") {
local = "127.0.0.1";
}
std::shared_ptr<TcpClientPort> socket = std::make_shared<TcpClientPort>();
socket->connect(local, 0, remoteIpAddr, remotePort);
return createConnection(nodeName, socket);
}
std::string MavLinkConnectionImpl::acceptTcp(std::shared_ptr<MavLinkConnection> parent, const std::string& nodeName, const std::string& localAddr, int listeningPort)
{
std::string local = localAddr;
close();
std::shared_ptr<TcpClientPort> socket = std::make_shared<TcpClientPort>();
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<MavLinkConnection> MavLinkConnectionImpl::connectSerial(const std::string& nodeName, const std::string& portName, int baudRate, const std::string& initString)
{
std::shared_ptr<SerialPort> serial = std::make_shared<SerialPort>();
int hr = serial->connect(portName.c_str(), baudRate);
if (hr != 0)
throw std::runtime_error(Utils::stringf("Could not open the serial port %s, error=%d", portName.c_str(), hr));
// send this right away just in case serial link is not already configured
if (initString.size() > 0) {
serial->write(reinterpret_cast<const uint8_t*>(initString.c_str()), static_cast<int>(initString.size()));
}
return createConnection(nodeName, serial);
}
void MavLinkConnectionImpl::startListening(std::shared_ptr<MavLinkConnection> parent, const std::string& nodeName, std::shared_ptr<Port> connectedPort)
{
name = nodeName;
con_ = parent;
if (port != connectedPort) {
close();
port = connectedPort;
}
closed = false;
Utils::cleanupThread(read_thread);
read_thread = std::thread{ &MavLinkConnectionImpl::readPackets, this };
Utils::cleanupThread(publish_thread_);
publish_thread_ = std::thread{ &MavLinkConnectionImpl::publishPackets, this };
}
// log every message that is "sent" using sendMessage.
void MavLinkConnectionImpl::startLoggingSendMessage(std::shared_ptr<MavLinkLog> log)
{
sendLog_ = log;
}
void MavLinkConnectionImpl::stopLoggingSendMessage()
{
sendLog_ = nullptr;
}
// log every message that is "sent" using sendMessage.
void MavLinkConnectionImpl::startLoggingReceiveMessage(std::shared_ptr<MavLinkLog> log)
{
receiveLog_ = log;
}
void MavLinkConnectionImpl::stopLoggingReceiveMessage()
{
receiveLog_ = nullptr;
}
void MavLinkConnectionImpl::close()
{
closed = true;
if (port != nullptr) {
port->close();
port = nullptr;
}
if (read_thread.joinable()) {
read_thread.join();
}
if (publish_thread_.joinable()) {
msg_available_.post();
publish_thread_.join();
}
sendLog_ = nullptr;
receiveLog_ = nullptr;
}
bool MavLinkConnectionImpl::isOpen()
{
return !closed;
}
int MavLinkConnectionImpl::getTargetComponentId()
{
return this->other_component_id;
}
int MavLinkConnectionImpl::getTargetSystemId()
{
return this->other_system_id;
}
uint8_t MavLinkConnectionImpl::getNextSequence()
{
std::lock_guard<std::mutex> guard(buffer_mutex);
return next_seq++;
}
void MavLinkConnectionImpl::ignoreMessage(uint8_t message_id)
{
ignored_messageids.insert(message_id);
}
void MavLinkConnectionImpl::sendMessage(const MavLinkMessage& m)
{
if (ignored_messageids.find(m.msgid) != ignored_messageids.end())
return;
if (closed) {
return;
}
{
MavLinkMessage msg;
::memcpy(&msg, &m, sizeof(MavLinkMessage));
prepareForSending(msg);
if (sendLog_ != nullptr) {
sendLog_->write(msg);
}
mavlink_message_t message;
message.compid = msg.compid;
message.sysid = msg.sysid;
message.len = msg.len;
message.checksum = msg.checksum;
message.magic = msg.magic;
message.incompat_flags = msg.incompat_flags;
message.compat_flags = msg.compat_flags;
message.seq = msg.seq;
message.msgid = msg.msgid;
::memcpy(message.signature, msg.signature, 13);
::memcpy(message.payload64, msg.payload64, PayloadSize * sizeof(uint64_t));
std::lock_guard<std::mutex> guard(buffer_mutex);
unsigned len = mavlink_msg_to_send_buffer(message_buf, &message);
/*
// hw-debug 打印要发送的mavlink报文内容, 132是距离传感器的ID
if (message.msgid == 132)
{
unsigned char *p=message_buf;
hw_extern_msg += Utils::stringf("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n"
"%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X\n",
p,p,p,p,p,p,p,p,p,p,
p,p,p,p,p,p,p,p,p,p,
p,p,p,p,p);
}
*/
try {
port->write(message_buf, len);
}
catch (std::exception& e) {
throw std::runtime_error(Utils::stringf("MavLinkConnectionImpl: Error sending message on connection '%s', details: %s", name.c_str(), e.what()));
}
}
{
std::lock_guard<std::mutex> guard(telemetry_mutex_);
telemetry_.messages_sent++;
}
}
int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg)
{
// as perhttps://github.com/mavlink/mavlink/blob/master/doc/MAVLink2.md
int seqno = getNextSequence();
bool mavlink1 = !supports_mavlink2_ && msg.protocol_version != 2;
bool signing = !mavlink1 && mavlink_status_.signing && (mavlink_status_.signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
uint8_t signature_len = signing ? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
uint8_t header_len = MAVLINK_CORE_HEADER_LEN + 1;
uint8_t buf;
if (mavlink1) {
msg.magic = MAVLINK_STX_MAVLINK1;
header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1;
}
else {
msg.magic = MAVLINK_STX;
}
msg.seq = seqno;
msg.incompat_flags = 0;
if (signing) {
msg.incompat_flags |= MAVLINK_IFLAG_SIGNED;
}
msg.compat_flags = 0;
// pack the payload buffer.
char* payload = reinterpret_cast<char*>(&msg.payload64);
int len = msg.len;
// calculate checksum
const mavlink_msg_entry_t* entry = mavlink_get_msg_entry(msg.msgid);
uint8_t crc_extra = 0;
int msglen = 0;
if (entry != nullptr) {
crc_extra = entry->crc_extra;
msglen = entry->min_msg_len;
}
if (msg.msgid == MavLinkTelemetry::kMessageId) {
msglen = MavLinkTelemetry::MessageLength; // mavlink doesn't know about our custom telemetry message.
}
if (len != msglen) {
if (mavlink1) {
throw std::runtime_error(Utils::stringf("Message length %d doesn't match expected length%d\n", len, msglen));
}
else {
// mavlink2 supports trimming the payload of trailing zeros so the messages
// are variable length as a result.
}
}
// mavlink2版本的payload信息是可以变长的，结尾的0可以被去掉，接收方会自动补全
// _mav_trim_payload函数就是用来去掉结尾0的，但是原来的参数传递错误了，传的是msglen：_mav_trim_payload(payload, msglen)
// 而msglen是最小长度，由此导致传递的数据不完整。
// 应该传递的是实际数据长度，改成_mav_trim_payload(payload, len)
len = mavlink1 ? msglen : _mav_trim_payload(payload, len);
msg.len = len;
// form the header as a byte array for the crc
buf = msg.magic;
buf = msg.len;
if (mavlink1) {
buf = msg.seq;
buf = msg.sysid;
buf = msg.compid;
buf = msg.msgid & 0xFF;
}
else {
buf = msg.incompat_flags;
buf = msg.compat_flags;
buf = msg.seq;
buf = msg.sysid;
buf = msg.compid;
buf = msg.msgid & 0xFF;
buf = (msg.msgid >> 8) & 0xFF;
buf = (msg.msgid >> 16) & 0xFF;
}
msg.checksum = crc_calculate(&buf, header_len - 1);
crc_accumulate_buffer(&msg.checksum, payload, msg.len);
crc_accumulate(crc_extra, &msg.checksum);
// these macros use old style cast.
STRICT_MODE_OFF
mavlink_ck_a(&msg) = (uint8_t)(msg.checksum & 0xFF);
mavlink_ck_b(&msg) = (uint8_t)(msg.checksum >> 8);
STRICT_MODE_ON
if (signing) {
mavlink_sign_packet(mavlink_status_.signing,
reinterpret_cast<uint8_t*>(msg.signature),
reinterpret_cast<const uint8_t*>(message_buf),
header_len,
reinterpret_cast<const uint8_t*>(payload),
msg.len,
reinterpret_cast<const uint8_t*>(payload) + msg.len);
}
return msg.len + header_len + 2 + signature_len;
}
void MavLinkConnectionImpl::sendMessage(const MavLinkMessageBase& msg)
{
MavLinkMessage m;
msg.encode(m);
sendMessage(m);
}
int MavLinkConnectionImpl::subscribe(MessageHandler handler)
{
MessageHandlerEntry entry{ static_cast<int>(listeners.size() + 1), handler };
std::lock_guard<std::mutex> guard(listener_mutex);
listeners.push_back(entry);
snapshot_stale = true;
return entry.id;
}
void MavLinkConnectionImpl::unsubscribe(int id)
{
std::lock_guard<std::mutex> guard(listener_mutex);
for (auto ptr = listeners.begin(), end = listeners.end(); ptr != end; ptr++) {
if ((*ptr).id == id) {
listeners.erase(ptr);
snapshot_stale = true;
break;
}
}
}
void MavLinkConnectionImpl::joinLeftSubscriber(std::shared_ptr<MavLinkConnection> remote, std::shared_ptr<MavLinkConnection> connection, const MavLinkMessage& msg)
{
unused(connection);
// forward messages from our connected node to the remote proxy.
if (supports_mavlink2_) {
// tell the remote connection to expect mavlink2 messages.
remote->pImpl->supports_mavlink2_ = true;
}
remote->sendMessage(msg);
}
void MavLinkConnectionImpl::joinRightSubscriber(std::shared_ptr<MavLinkConnection> connection, const MavLinkMessage& msg)
{
unused(connection);
// forward messages from remote proxy to local connected node
this->sendMessage(msg);
}
void MavLinkConnectionImpl::join(std::shared_ptr<MavLinkConnection> remote, bool subscribeToLeft, bool subscribeToRight)
{
if (subscribeToLeft)
this->subscribe(std::bind(&MavLinkConnectionImpl::joinLeftSubscriber, this, remote, std::placeholders::_1, std::placeholders::_2));
if (subscribeToRight)
remote->subscribe(std::bind(&MavLinkConnectionImpl::joinRightSubscriber, this, std::placeholders::_1, std::placeholders::_2));
}
void MavLinkConnectionImpl::readPackets()
{
//CurrentThread::setMaximumPriority();
CurrentThread::setThreadName("MavLinkThread");
std::shared_ptr<Port> safePort = this->port;
mavlink_message_t msg;
mavlink_message_t msgBuffer; // intermediate state.
const int MAXBUFFER = 512;
uint8_t* buffer = new uint8_t;
mavlink_intermediate_status_.parse_state = MAVLINK_PARSE_STATE_IDLE;
int channel = 0;
int hr = 0;
while (hr == 0 && con_ != nullptr && !closed) {
int read = 0;
if (safePort->isClosed()) {
// hmmm, wait till it is opened?
std::this_thread::sleep_for(std::chrono::milliseconds(10));
continue;
}
int count = safePort->read(buffer, MAXBUFFER);
if (count <= 0) {
// error? well let's try again, but we should be careful not to spin too fast and kill the CPU
std::this_thread::sleep_for(std::chrono::milliseconds(1));
continue;
}
for (int i = 0; i < count; i++) {
uint8_t frame_state = mavlink_frame_char_buffer(&msgBuffer, &mavlink_intermediate_status_, buffer, &msg, &mavlink_status_);
if (frame_state == MAVLINK_FRAMING_INCOMPLETE) {
continue;
}
else if (frame_state == MAVLINK_FRAMING_BAD_CRC) {
std::lock_guard<std::mutex> guard(telemetry_mutex_);
telemetry_.crc_errors++;
}
else if (frame_state == MAVLINK_FRAMING_OK) {
// pick up the sysid/compid of the remote node we are connected to.
if (other_system_id == -1) {
other_system_id = msg.sysid;
other_component_id = msg.compid;
}
if (mavlink_intermediate_status_.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
// then this is a mavlink 1 message
}
else if (!supports_mavlink2_) {
// then this mavlink sender supports mavlink 2
supports_mavlink2_ = true;
}
if (con_ != nullptr && !closed) {
{
std::lock_guard<std::mutex> guard(telemetry_mutex_);
telemetry_.messages_received++;
}
// queue event for publishing.
{
std::lock_guard<std::mutex> guard(msg_queue_mutex_);
MavLinkMessage message;
message.compid = msg.compid;
message.sysid = msg.sysid;
message.len = msg.len;
message.checksum = msg.checksum;
message.magic = msg.magic;
message.incompat_flags = msg.incompat_flags;
message.compat_flags = msg.compat_flags;
message.seq = msg.seq;
message.msgid = msg.msgid;
message.protocol_version = supports_mavlink2_ ? 2 : 1;
::memcpy(message.signature, msg.signature, 13);
::memcpy(message.payload64, msg.payload64, PayloadSize * sizeof(uint64_t));
msg_queue_.push(message);
}
if (waiting_for_msg_) {
msg_available_.post();
}
}
}
else {
std::lock_guard<std::mutex> guard(telemetry_mutex_);
telemetry_.crc_errors++;
}
}
} //while
delete[] buffer;
} //readPackets
void MavLinkConnectionImpl::drainQueue()
{
MavLinkMessage message;
bool hasMsg = true;
while (hasMsg) {
hasMsg = false;
{
std::lock_guard<std::mutex> guard(msg_queue_mutex_);
if (!msg_queue_.empty()) {
message = msg_queue_.front();
msg_queue_.pop();
hasMsg = true;
}
}
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) {
// this is tricky, the clear has to be done outside the lock because it is destructing the handlers
// and the handler might try and call unsubscribe, which needs to be able to grab the lock, otherwise
// we would get a deadlock.
snapshot.clear();
std::lock_guard<std::mutex> guard(listener_mutex);
snapshot = listeners;
snapshot_stale = false;
}
auto end = snapshot.end();
if (message.msgid == static_cast<uint8_t>(MavLinkMessageIds::MAVLINK_MSG_ID_AUTOPILOT_VERSION)) {
MavLinkAutopilotVersion cap;
cap.decode(message);
if ((cap.capabilities & MAV_PROTOCOL_CAPABILITY_MAVLINK2) != 0) {
this->supports_mavlink2_ = true;
}
}
auto startTime = std::chrono::system_clock::now();
std::shared_ptr<MavLinkConnection> sharedPtr = std::shared_ptr<MavLinkConnection>(this->con_);
for (auto ptr = snapshot.begin(); ptr != end; ptr++) {
try {
(*ptr).handler(sharedPtr, message);
}
catch (std::exception& e) {
Utils::log(Utils::stringf("MavLinkConnectionImpl: Error handling message %d on connection '%s', details: %s",
message.msgid,
name.c_str(),
e.what()),
Utils::kLogLevelError);
}
}
{
auto endTime = std::chrono::system_clock::now();
auto diff = endTime - startTime;
long microseconds = static_cast<long>(std::chrono::duration_cast<std::chrono::microseconds>(diff).count());
std::lock_guard<std::mutex> guard(telemetry_mutex_);
telemetry_.messages_handled++;
telemetry_.handler_microseconds += microseconds;
}
}
}
void MavLinkConnectionImpl::publishPackets()
{
//CurrentThread::setMaximumPriority();
CurrentThread::setThreadName("MavLinkThread");
publish_thread_id_ = std::this_thread::get_id();
while (!closed) {
drainQueue();
waiting_for_msg_ = true;
msg_available_.wait();
waiting_for_msg_ = false;
}
}
bool MavLinkConnectionImpl::isPublishThread() const
{
return std::this_thread::get_id() == publish_thread_id_;
}
void MavLinkConnectionImpl::getTelemetry(MavLinkTelemetry& result)
{
std::lock_guard<std::mutex> guard(telemetry_mutex_);
result = telemetry_;
// reset counters
telemetry_.crc_errors = 0;
telemetry_.handler_microseconds = 0;
telemetry_.messages_handled = 0;
telemetry_.messages_received = 0;
telemetry_.messages_sent = 0;
if (telemetry_.wifiInterfaceName != nullptr) {
telemetry_.wifi_rssi = port->getRssi(telemetry_.wifiInterfaceName);
}
}MavLinkMultirotorApi.hpp
// Copyright (c) Microsoft Corporation. All rights reserved.// Licensed under the MIT License.#ifndef msr_airlib_MavLinkDroneController_hpp#define msr_airlib_MavLinkDroneController_hpp#include "MavLinkConnection.hpp"#include "MavLinkMessages.hpp"#include "MavLinkNode.hpp"#include "MavLinkVehicle.hpp"#include "MavLinkVideoStream.hpp"#include #include #include #include #include #include #include #include #include "common/AirSimSettings.hpp"#include "common/Common.hpp"#include "common/CommonStructs.hpp"#include "common/PidController.hpp"#include "common/VectorMath.hpp"#include "common/common_utils/FileSystem.hpp"#include "common/common_utils/SmoothingFilter.hpp"#include "common/common_utils/Timer.hpp"#include "physics/World.hpp"#include "sensors/SensorCollection.hpp"#include "vehicles/multirotor/api/MultirotorApiBase.hpp"//sensors#include "sensors/barometer/BarometerBase.hpp"#include "sensors/distance/DistanceSimple.hpp"#include "sensors/gps/GpsBase.hpp"#include "sensors/imu/ImuBase.hpp"#include "sensors/magnetometer/MagnetometerBase.hpp"// hw-modify 调试信息开关#define HW_DEBUG_MSG 0// hw-modify 目前只会在本文件中用addStatusMessage进行打印, 因此其他所有模块的打开都由本文件负责. 临时修改,后续要完善// 在MavLinkConnectionImpl.cpp中定义了全局变量hw_extern_msg, 其他模块向该变量中写入要打印的内容// 本模块的update函数中会循环打印该字符串的值. 每次打印后都会清空该字符串extern std::string hw_extern_msg;namespace msr{namespace airlib{ class MavLinkMultirotorApi : public MultirotorApiBase { public: //methods virtual ~MavLinkMultirotorApi() { closeAllConnection(); if (this->connect_thread_.joinable()) { this->connect_thread_.join(); } if (this->telemetry_thread_.joinable()) { this->telemetry_thread_.join(); } } //non-base interface specific to MavLinKDroneController void initialize(const AirSimSettings::MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation) { connection_info_ = connection_info; sensors_ = sensors; is_simulation_mode_ = is_simulation; lock_step_enabled_ = connection_info.lock_step; try { openAllConnections(); is_ready_ = true; } catch (std::exception& ex) { is_ready_ = false; is_ready_message_ = Utils::stringf("Failed to connect: %s", ex.what()); } } Pose getMocapPose() { std::lock_guard guard(mocap_pose_mutex_); return mocap_pose_; } virtual const SensorCollection& getSensors() const override { return *sensors_; } //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. world_ = nullptr; for (UpdatableObject* container = this->getParent(); container != nullptr; container = container->getParent()) { if (container->getName() == "World") { // cool beans! // note: cannot use dynamic_cast because Unreal builds with /GR- for some unknown reason... world_ = static_cast(container); } } MultirotorApiBase::resetImplementation(); was_reset_ = true; } unsigned long long getSimTime() { // This ensures HIL_SENSOR and HIL_GPS have matching clocks. if (lock_step_active_) { if (sim_time_us_ == 0) { sim_time_us_ = clock()->nowNanos() / 1000; } return sim_time_us_; } else { return clock()->nowNanos() / 1000; } } void advanceTime() { sim_time_us_ = clock()->nowNanos() / 1000; } //update sensors in PX4 stack virtual void update() override { // hw-modify 打印全局字符串,打印完成后清空 if (hw_extern_msg.length()) { addStatusMessage(hw_extern_msg); hw_extern_msg.clear(); } try { auto now = clock()->nowNanos() / 1000; MultirotorApiBase::update(); if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_) return; { std::lock_guard guard(telemetry_mutex_); update_count_++; } if (lock_step_active_) { if (last_update_time_ + 1000000 < now) { // if 1 second passes then something is terribly wrong, reset lockstep mode lock_step_active_ = false; { std::lock_guard guard(telemetry_mutex_); lock_step_resets_++; } addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode"); } else if (!received_actuator_controls_) { // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage. return; } } last_update_time_ = now; { std::lock_guard guard(telemetry_mutex_); hil_sensor_count_++; } advanceTime(); //send sensor updates const auto& imu_output = getImuData(""); const auto& mag_output = getMagnetometerData(""); const auto& baro_output = getBarometerData(""); sendHILSensor(imu_output.linear_acceleration, imu_output.angular_velocity, mag_output.magnetic_field_body, baro_output.pressure * 0.01f /*Pa to Millibar */, baro_output.altitude); sendSystemTime(); #if 0 const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance); if (count_distance_sensors != 0) { const auto* distance_sensor = static_cast( sensors_->getByType(SensorBase::SensorType::Distance)); // Don't send the data if sending to external controller is disabled in settings if (distance_sensor && distance_sensor->getParams().external_controller) { const auto& distance_output = distance_sensor->getOutput(); sendDistanceSensor(distance_output.min_distance * 100, //m -> cm distance_output.max_distance * 100, //m -> cm distance_output.distance * 100, //m-> cm 0, //sensor type: //TODO: allow changing in settings? 77, //sensor id, //TODO: should this be something real? distance_output.relative_pose.orientation); //TODO: convert from radians to degrees? } } #else // hw-modify 遍历所有传感器并发送到px4，以前的逻辑只处理了第一个传感器 const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance); if (count_distance_sensors != 0) { for (uint i=0; igetByType(SensorBase::SensorType::Distance, i)); // Don't send the data if sending to external controller is disabled in settings if (distance_sensor && distance_sensor->getParams().external_controller) { const auto& distance_output = distance_sensor->getOutput(); // 调试信息 #if (HW_DEBUG_MSG) auto msg = Utils::stringf("distance_sensor(%d): min=%f, max=%f, distance=%f, 4.w=%f, 4.x=%f, 4.y=%f, 4.z=%f", i, distance_output.min_distance, distance_output.max_distance, distance_output.distance, distance_output.relative_pose.orientation.w(), distance_output.relative_pose.orientation.x(), distance_output.relative_pose.orientation.y(), distance_output.relative_pose.orientation.z()); addStatusMessage(msg); msg = Utils::stringf("Param: min=%f, max=%f, x=%f, y=%f, z=%f, roll=%f, pitch=%f, yaw=%f\n\n", distance_sensor->getParams().min_distance, distance_sensor->getParams().max_distance, distance_sensor->getParams().relative_pose.position.x(),distance_sensor->getParams().relative_pose.position.y(),distance_sensor->getParams().relative_pose.position.z(), distance_sensor->getParams().relative_pose.orientation.x(),distance_sensor->getParams().relative_pose.orientation.y(),distance_sensor->getParams().relative_pose.orientation.z()); addStatusMessage(msg); #endif sendDistanceSensor(distance_output.min_distance * 100, //m -> cm distance_output.max_distance * 100, //m -> cm distance_output.distance * 100, //m-> cm 0, //sensor type, https://mavlink.io/zh/messages/common.html#MAV_DISTANCE_SENSOR 描述了支持的传感器类型 (uint8_t)i+1, // sensor id, 使用i+1作为传感器索引，区分不同传感器 distance_output.relative_pose.orientation); // 直接使用当前的sin半角数值，不用转换成角度单位的值 } } } #endif const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps); if (count_gps_sensors != 0) { const auto& gps_output = getGpsData(""); //send GPS if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) { last_gps_time_ = gps_output.gnss.time_utc; Vector3r gps_velocity = gps_output.gnss.velocity; Vector3r gps_velocity_xy = gps_velocity; gps_velocity_xy.z() = 0; float gps_cog; if (Utils::isApproximatelyZero(gps_velocity.y(), 1E-2f) && Utils::isApproximatelyZero(gps_velocity.x(), 1E-2f)) gps_cog = 0; else gps_cog = Utils::radiansToDegrees(atan2(gps_velocity.y(), gps_velocity.x())); if (gps_cog < 0) gps_cog += 360; 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); } } auto end = clock()->nowNanos() / 1000; { std::lock_guard guard(telemetry_mutex_); update_time_ += (end - now); } } catch (std::exception& e) { addStatusMessage("Exception sending messages to vehicle"); addStatusMessage(e.what()); disconnect(); connect(); // re-start a new connection so PX4 can be restarted and AirSim will happily continue on. } //must be done at the end if (was_reset_) was_reset_ = false; } virtual bool isReady(std::string& message) const override { if (!is_ready_ && is_ready_message_.size() > 0) { message = is_ready_message_; } return is_ready_; } virtual bool canArm() const override { return is_ready_ && has_gps_lock_; } //TODO: this method can't be const yet because it clears previous messages virtual void getStatusMessages(std::vector& messages) override { updateState(); //clear param messages.clear(); //move messages from private vars to param std::lock_guard guard(status_text_mutex_); while (!status_messages_.empty()) { messages.push_back(status_messages_.front()); status_messages_.pop(); } } virtual Kinematics::State getKinematicsEstimated() const override { updateState(); Kinematics::State state; //TODO: reduce code duplication in getPosition() etc methods? state.pose.position = Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z); state.pose.orientation = VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw); state.twist.linear = Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z); state.twist.angular = Vector3r(current_state_.attitude.roll_rate, current_state_.attitude.pitch_rate, current_state_.attitude.yaw_rate); state.accelerations.linear = Vector3r(current_state_.local_est.acc.x, current_state_.local_est.acc.y, current_state_.local_est.acc.z); //TODO: how do we get angular acceleration? return state; } virtual bool isApiControlEnabled() const override { return is_api_control_enabled_; } virtual void enableApiControl(bool is_enabled) override { checkValidVehicle(); if (is_enabled) { mav_vehicle_->requestControl(); is_api_control_enabled_ = true; } else { mav_vehicle_->releaseControl(); is_api_control_enabled_ = false; } } virtual Vector3r getPosition() const override { updateState(); return Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z); } virtual Vector3r getVelocity() const override { updateState(); return Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z); } virtual Quaternionr getOrientation() const override { updateState(); return VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw); } virtual LandedState getLandedState() const override { updateState(); return current_state_.controls.landed ? LandedState::Landed : LandedState::Flying; } virtual real_T getActuation(unsigned int rotor_index) const override { if (!is_simulation_mode_) throw std::logic_error("Attempt to read motor controls while not in simulation mode"); std::lock_guard guard(hil_controls_mutex_); return rotor_controls_; } virtual size_t getActuatorCount() const override { return RotorControlsCount; } virtual bool armDisarm(bool arm) override { SingleCall lock(this); checkValidVehicle(); bool rc = false; if (arm) { float timeout_sec = 10; waitForHomeLocation(timeout_sec); waitForStableGroundPosition(timeout_sec); } mav_vehicle_->armDisarm(arm).wait(10000, &rc); 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(); log_ = nullptr; } try { 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_file_name_ = fullpath; 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_); } start_telemtry_thread(); } catch (std::exception& ex) { addStatusMessage(std::string("Opening log file failed: ") + ex.what()); } } } } void onDisarmed() { if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) { auto con = mav_vehicle_->getConnection(); if (con != nullptr) { con->stopLoggingSendMessage(); con->stopLoggingReceiveMessage(); } if (connection_ != nullptr) { connection_->stopLoggingSendMessage(); connection_->stopLoggingReceiveMessage(); } } if (log_ != nullptr) { addStatusMessage(Utils::stringf("Closing log file: %s", log_file_name_.c_str())); log_->close(); log_ = nullptr; } } void waitForHomeLocation(float timeout_sec) { if (!current_state_.home.is_set) { addStatusMessage("Waiting for valid GPS home location..."); if (!waitForFunction([&]() { return current_state_.home.is_set; }, timeout_sec) .isComplete()) { throw VehicleMoveException("Vehicle does not have a valid GPS home location"); } } } void waitForStableGroundPosition(float timeout_sec) { // wait for ground stabilization if (ground_variance_ > GroundTolerance) { addStatusMessage("Waiting for z-position to stabilize..."); if (!waitForFunction([&]() { return ground_variance_ takeoff(z, 0.0f /* pitch */, yaw).wait(static_cast(timeout_sec * 1000), &rc)) { throw VehicleMoveException("TakeOff command - timeout waiting for response"); } if (!rc) { throw VehicleMoveException("TakeOff command rejected by drone"); } if (timeout_sec
页:
[1]