博客地址：https://www.cnblogs.com/zylyehuo/
参考链接： AirSim+PX4 无人机自动避障详细教程
AirSim 配置文件修改
代码修改完成后，进入 AirSim 根目录运行如下代码进行编译
bash
./setup.sh
./build.sh运行完成后，将 Plugins 文件夹重新复制到 UE4 根目录下
MavLinkConnectionImpl.cpp
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[0],p[1],p[2],p[3],p[4],p[5],p[6],p[7],p[8],p[9],
p[10],p[11],p[12],p[13],p[14],p[15],p[16],p[17],p[18],p[19],
p[20],p[21],p[22],p[23],p[24]);
}
*/
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 per https://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[MAVLINK_CORE_HEADER_LEN + 1];
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[0]);
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[0] = msg.magic;
buf[1] = msg.len;
if (mavlink1) {
buf[2] = msg.seq;
buf[3] = msg.sysid;
buf[4] = msg.compid;
buf[5] = msg.msgid & 0xFF;
}
else {
buf[2] = msg.incompat_flags;
buf[3] = msg.compat_flags;
buf[4] = msg.seq;
buf[5] = msg.sysid;
buf[6] = msg.compid;
buf[7] = msg.msgid & 0xFF;
buf[8] = (msg.msgid >> 8) & 0xFF;
buf[9] = (msg.msgid >> 16) & 0xFF;
}
msg.checksum = crc_calculate(&buf[1], 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[MAXBUFFER];
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[i], &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
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 <exception>
#include <memory>
#include <mutex>
#include <queue>
#include <string>
#include <thread>
#include <tuple>
#include <vector>
#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<std::mutex> 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<World*>(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<std::mutex> 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<std::mutex> 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<std::mutex> 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<const DistanceSimple*>(
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; i<count_distance_sensors; i++)
{
const auto* distance_sensor = static_cast<const DistanceSimple*>(
sensors_->getByType(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<std::mutex> 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<std::string>& messages) override
{
updateState();
//clear param
messages.clear();
//move messages from private vars to param
std::lock_guard<std::mutex> 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<std::mutex> guard(hil_controls_mutex_);
return rotor_controls_[rotor_index];
}
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<mavlinkcom::MavLinkFileLog>();
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_ <= GroundTolerance;
},
timeout_sec)
.isComplete()) {
auto msg = Utils::stringf("Ground is not stable, variance is %f", ground_variance_);
throw VehicleMoveException(msg);
}
}
}
virtual bool takeoff(float timeout_sec) override
{
SingleCall lock(this);
checkValidVehicle();
waitForHomeLocation(timeout_sec);
waitForStableGroundPosition(timeout_sec);
bool rc = false;
auto vec = getPosition();
auto yaw = current_state_.attitude.yaw;
float z = vec.z() + getTakeoffZ();
if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, yaw).wait(static_cast<int>(timeout_sec * 1000), &rc)) {
throw VehicleMoveException("TakeOff command - timeout waiting for response");
}
if (!rc) {
throw VehicleMoveException("TakeOff command rejected by drone");
}
if (timeout_sec <= 0)
return true; // client doesn't want to wait.
return waitForZ(timeout_sec, z, getDistanceAccuracy());
}
virtual bool land(float timeout_sec) override
{
SingleCall lock(this);
//TODO: bugbug: really need a downward pointing distance to ground sensor to do this properly, for now
//we assume the ground is relatively flat an we are landing roughly at the home altitude.
updateState();
checkValidVehicle();
if (current_state_.home.is_set) {
bool rc = false;
if (!mav_vehicle_->land(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.home.global_pos.alt).wait(10000, &rc)) {
throw VehicleMoveException("Landing command - timeout waiting for response from drone");
}
else if (!rc) {
throw VehicleMoveException("Landing command rejected by drone");
}
}
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...");
}
const auto& waiter = waitForFunction([&]() {
updateState();
return current_state_.controls.landed;
},
timeout_sec);
// Wait for landed state (or user cancellation)
if (!waiter.isComplete()) {
throw VehicleMoveException("Drone hasn't reported a landing state");
}
return waiter.isComplete();
}
virtual bool goHome(float timeout_sec) override
{
SingleCall lock(this);
checkValidVehicle();
bool rc = false;
if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait(
static_cast<int>(timeout_sec) * 1000, &rc)) {
throw VehicleMoveException("goHome - timeout waiting for response from drone");
}
return rc;
}
virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain,
const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) override
{
SingleTaskCall lock(this);
unused(adaptive_lookahead);
unused(lookahead);
unused(drivetrain);
checkValidVehicle();
// save current manual, cruise, and max velocity parameters
bool result = false;
mavlinkcom::MavLinkParameter manual_velocity_parameter, cruise_velocity_parameter, max_velocity_parameter;
result = mav_vehicle_->getParameter("MPC_VEL_MANUAL").wait(1000, &manual_velocity_parameter);
result = result && mav_vehicle_->getParameter("MPC_XY_CRUISE").wait(1000, &cruise_velocity_parameter);
result = result && mav_vehicle_->getParameter("MPC_XY_VEL_MAX").wait(1000, &max_velocity_parameter);
if (result) {
// set max velocity parameter
mavlinkcom::MavLinkParameter p;
p.name = "MPC_XY_VEL_MAX";
p.value = velocity;
mav_vehicle_->setParameter(p).wait(1000, &result);
if (result) {
const Vector3r& goal_pos = Vector3r(x, y, z);
Vector3r goal_dist_vect;
float goal_dist;
Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken());
while (!waiter.isComplete()) {
goal_dist_vect = getPosition() - goal_pos;
const Vector3r& goal_normalized = goal_dist_vect.normalized();
goal_dist = goal_dist_vect.dot(goal_normalized);
if (goal_dist > getDistanceAccuracy()) {
moveToPositionInternal(goal_pos, yaw_mode);
//sleep for rest of the cycle
if (!waiter.sleep())
return false;
}
else {
waiter.complete();
}
}
// reset manual, cruise, and max velocity parameters
bool result_temp = false;
mav_vehicle_->setParameter(manual_velocity_parameter).wait(1000, &result);
mav_vehicle_->setParameter(cruise_velocity_parameter).wait(1000, &result_temp);
result = result && result_temp;
mav_vehicle_->setParameter(max_velocity_parameter).wait(1000, &result_temp);
result = result && result_temp;
return result && waiter.isComplete();
}
}
return result;
}
virtual bool hover() override
{
SingleCall lock(this);
bool rc = false;
checkValidVehicle();
mavlinkcom::AsyncResult<bool> result = mav_vehicle_->loiter();
//auto start_time = std::chrono::system_clock::now();
while (!getCancelToken().isCancelled()) {
if (result.wait(100, &rc)) {
break;
}
}
return rc;
}
virtual GeoPoint getHomeGeoPoint() const override
{
updateState();
if (current_state_.home.is_set)
return GeoPoint(current_state_.home.global_pos.lat, current_state_.home.global_pos.lon, current_state_.home.global_pos.alt);
else
return GeoPoint(Utils::nan<double>(), Utils::nan<double>(), Utils::nan<float>());
}
virtual GeoPoint getGpsLocation() const override
{
updateState();
return GeoPoint(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.global_est.pos.alt);
}
virtual void sendTelemetry(float last_interval = -1) override
{
if (connection_ == nullptr || mav_vehicle_ == nullptr) {
return;
}
// This method is called at high frequence from MultirotorPawnSimApi::updateRendering.
mavlinkcom::MavLinkTelemetry data;
connection_->getTelemetry(data);
if (data.messages_received == 0) {
if (!hil_message_timer_.started()) {
hil_message_timer_.start();
}
else if (hil_message_timer_.seconds() > messageReceivedTimeout) {
addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again");
hil_message_timer_.stop();
}
}
else {
hil_message_timer_.stop();
}
}
void writeTelemetry(float last_interval = -1)
{
auto proxy = logviewer_proxy_;
auto log = log_;
if ((logviewer_proxy_ == nullptr && log_ == nullptr)) {
return;
}
mavlinkcom::MavLinkTelemetry data;
connection_->getTelemetry(data);
// listen to the other mavlink connection also
auto mavcon = mav_vehicle_->getConnection();
if (mavcon != connection_) {
mavlinkcom::MavLinkTelemetry gcs;
mavcon->getTelemetry(gcs);
data.handler_microseconds += gcs.handler_microseconds;
data.messages_handled += gcs.messages_handled;
data.messages_received += gcs.messages_received;
data.messages_sent += gcs.messages_sent;
if (gcs.messages_received == 0) {
if (!gcs_message_timer_.started()) {
gcs_message_timer_.start();
}
else if (gcs_message_timer_.seconds() > messageReceivedTimeout) {
addStatusMessage("not receiving any messages from GCS port, please restart your SITL node and try again");
}
}
else {
gcs_message_timer_.stop();
}
}
data.render_time = static_cast<int64_t>(last_interval * 1000000); // microseconds
{
std::lock_guard<std::mutex> guard(telemetry_mutex_);
uint32_t average_delay = 0;
uint32_t average_update = 0;
if (hil_sensor_count_ != 0) {
average_delay = actuator_delay_ / hil_sensor_count_;
average_update = static_cast<uint32_t>(update_time_ / hil_sensor_count_);
}
data.update_rate = update_count_;
data.sensor_rate = hil_sensor_count_;
data.actuation_delay = average_delay;
data.lock_step_resets = lock_step_resets_;
data.update_time = average_update;
// reset the counters we just captured.
update_count_ = 0;
hil_sensor_count_ = 0;
actuator_delay_ = 0;
update_time_ = 0;
}
if (proxy != nullptr) {
proxy->sendMessage(data);
}
if (log != nullptr) {
mavlinkcom::MavLinkMessage msg;
msg.magic = MAVLINK_STX_MAVLINK1;
data.encode(msg);
msg.update_checksum();
// disk I/O is unpredictable, so we have to get it out of the update loop
// which is why this thread exists.
log->write(msg);
}
}
void start_telemtry_thread()
{
if (this->telemetry_thread_.joinable()) {
this->telemetry_thread_.join();
}
// reset the counters we use for telemetry.
update_count_ = 0;
hil_sensor_count_ = 0;
actuator_delay_ = 0;
this->telemetry_thread_ = std::thread(&MavLinkMultirotorApi::telemtry_thread, this);
}
void telemtry_thread()
{
while (log_ != nullptr) {
std::this_thread::sleep_for(std::chrono::seconds(1));
writeTelemetry(1);
}
}
virtual float getCommandPeriod() const override
{
return 1.0f / 50; //1 period of 50hz
}
virtual float getTakeoffZ() const override
{
// pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe
// enough to get out of the backwash turbulence. Negative due to NED coordinate system.
return -3.0f;
}
virtual float getDistanceAccuracy() const override
{
return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled
}
protected: //methods
virtual void setControllerGains(uint8_t controllerType, const vector<float>& kp, const vector<float>& ki, const vector<float>& kd) override
{
unused(controllerType);
unused(kp);
unused(ki);
unused(kd);
Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo);
}
virtual void commandMotorPWMs(float front_right_pwm, float front_left_pwm, float rear_right_pwm, float rear_left_pwm) override
{
unused(front_right_pwm);
unused(front_left_pwm);
unused(rear_right_pwm);
unused(rear_left_pwm);
Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo);
}
virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override
{
if (target_height_ != -z) {
// these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best
// control over thrust to achieve minimal over/under shoot in a reasonable amount of time, but it has not
// been tested on a real drone outside jMavSim, so it may need recalibrating...
thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
target_height_ = -z;
}
checkValidVehicle();
auto state = mav_vehicle_->getVehicleState();
float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust);
}
virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override
{
if (target_height_ != -z) {
thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
target_height_ = -z;
}
checkValidVehicle();
auto state = mav_vehicle_->getVehicleState();
float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, thrust);
}
virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override
{
checkValidVehicle();
mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle);
}
virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override
{
checkValidVehicle();
mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, throttle);
}
virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override
{
if (target_height_ != -z) {
thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
target_height_ = -z;
}
checkValidVehicle();
auto state = mav_vehicle_->getVehicleState();
float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, thrust);
}
virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override
{
checkValidVehicle();
mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, throttle);
}
virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
{
checkValidVehicle();
float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
mav_vehicle_->moveByLocalVelocity(vx, vy, vz, !yaw_mode.is_rate, yaw);
}
virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override
{
checkValidVehicle();
float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
mav_vehicle_->moveByLocalVelocityWithAltHold(vx, vy, z, !yaw_mode.is_rate, yaw);
}
virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override
{
checkValidVehicle();
float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw);
}
//TODO: decouple MultirotorApiBase, VehicalParams and SafetyEval
virtual const MultirotorApiParams& getMultirotorApiParams() const override
{
//defaults are good for PX4 generic quadcopter.
static const MultirotorApiParams vehicle_params_;
return vehicle_params_;
}
virtual void beforeTask() override
{
startOffboardMode();
}
virtual void afterTask() override
{
endOffboardMode();
}
public:
class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog
{
public:
MavLinkLogViewerLog(std::shared_ptr<mavlinkcom::MavLinkNode> proxy)
{
proxy_ = proxy;
}
~MavLinkLogViewerLog()
{
proxy_ = nullptr;
}
void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override
{
if (proxy_ != nullptr) {
unused(timestamp);
mavlinkcom::MavLinkMessage copy;
::memcpy(©, &msg, sizeof(mavlinkcom::MavLinkMessage));
try {
proxy_->sendMessage(copy);
}
catch (std::exception&) {
failures++;
if (failures == 100) {
// hmmm, doesn't like the proxy, bad socket perhaps, so stop trying...
proxy_ = nullptr;
}
}
}
}
private:
std::shared_ptr<mavlinkcom::MavLinkNode> proxy_;
int failures = 0;
};
protected: //methods
virtual void connect()
{
if (!connecting_) {
connecting_ = true;
if (this->connect_thread_.joinable()) {
this->connect_thread_.join();
}
this->connect_thread_ = std::thread(&MavLinkMultirotorApi::connect_thread, this);
}
}
virtual void disconnect()
{
addStatusMessage("Disconnecting mavlink vehicle");
connected_ = false;
connecting_ = false;
if (is_armed_) {
// close the telemetry log.
is_armed_ = false;
onDisarmed();
}
if (connection_ != nullptr) {
if (is_hil_mode_set_ && mav_vehicle_ != nullptr) {
setNormalMode();
}
connection_->stopLoggingSendMessage();
connection_->stopLoggingReceiveMessage();
connection_->close();
}
if (hil_node_ != nullptr) {
hil_node_->close();
}
if (mav_vehicle_ != nullptr) {
auto c = mav_vehicle_->getConnection();
if (c != nullptr) {
c->stopLoggingSendMessage();
c->stopLoggingReceiveMessage();
}
mav_vehicle_->close();
mav_vehicle_ = nullptr;
}
if (video_server_ != nullptr)
video_server_->close();
if (logviewer_proxy_ != nullptr) {
logviewer_proxy_->close();
logviewer_proxy_ = nullptr;
}
if (logviewer_out_proxy_ != nullptr) {
logviewer_out_proxy_->close();
logviewer_out_proxy_ = nullptr;
}
if (qgc_proxy_ != nullptr) {
qgc_proxy_->close();
qgc_proxy_ = nullptr;
}
resetState();
}
void connect_thread()
{
addStatusMessage("Waiting for mavlink vehicle...");
connecting_ = true;
createMavConnection(connection_info_);
if (mav_vehicle_ != nullptr) {
connectToLogViewer();
connectToQGC();
}
if (connecting_) {
// only set connected if we haven't already been told to disconnect.
connecting_ = false;
connected_ = true;
}
}
virtual void close()
{
disconnect();
}
void closeAllConnection()
{
close();
}
private: //methods
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();
}
void getMocapPose(Vector3r& position, Quaternionr& orientation) const
{
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];
}
//TODO: this method used to send collision to external sim. Do we still need this?
void sendCollision(float normalX, float normalY, float normalZ)
{
checkValidVehicle();
mavlinkcom::MavLinkCollision collision{};
collision.src = 1; //provider of data is MavLink system in id field
collision.id = mav_vehicle_->getLocalSystemId();
collision.action = static_cast<uint8_t>(mavlinkcom::MAV_COLLISION_ACTION::MAV_COLLISION_ACTION_REPORT);
collision.threat_level = static_cast<uint8_t>(mavlinkcom::MAV_COLLISION_THREAT_LEVEL::MAV_COLLISION_THREAT_LEVEL_NONE);
// we are abusing these fields, passing the angle of the object we hit, so that jMAVSim knows how to bounce off.
collision.time_to_minimum_delta = normalX;
collision.altitude_minimum_delta = normalY;
collision.horizontal_minimum_delta = normalZ;
mav_vehicle_->sendMessage(collision);
}
//TODO: do we still need this method?
bool hasVideoRequest()
{
mavlinkcom::MavLinkVideoServer::MavLinkVideoRequest image_req;
return video_server_->hasVideoRequest(image_req);
}
//TODO: do we still need this method?
void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height)
{
const int MAVLINK_DATA_STREAM_IMG_PNG = 6;
video_server_->sendFrame(data, length, width, height, MAVLINK_DATA_STREAM_IMG_PNG, 0);
}
//put PX4 in normal mode (i.e. non-simulation mode)
void setNormalMode()
{
if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) {
// remove MAV_MODE_FLAG_HIL_ENABLED flag from current mode
std::lock_guard<std::mutex> guard(set_mode_mutex_);
int mode = mav_vehicle_->getVehicleState().mode;
mode &= ~static_cast<int>(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
mavlinkcom::MavCmdDoSetMode cmd;
cmd.command = static_cast<uint16_t>(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE);
cmd.Mode = static_cast<float>(mode);
mav_vehicle_->sendCommand(cmd);
is_hil_mode_set_ = false;
}
}
//put PX4 in simulation mode
void setHILMode()
{
if (!is_simulation_mode_)
throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode");
checkValidVehicle();
// add MAV_MODE_FLAG_HIL_ENABLED flag to current mode
std::lock_guard<std::mutex> guard(set_mode_mutex_);
int mode = mav_vehicle_->getVehicleState().mode;
mode |= static_cast<int>(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
mavlinkcom::MavCmdDoSetMode cmd;
cmd.command = static_cast<uint16_t>(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE);
cmd.Mode = static_cast<float>(mode);
mav_vehicle_->sendCommand(cmd);
is_hil_mode_set_ = true;
}
bool startOffboardMode()
{
checkValidVehicle();
try {
mav_vehicle_->requestControl();
}
catch (std::exception& ex) {
ensureSafeMode();
addStatusMessage(std::string("Request control failed: ") + ex.what());
return false;
}
return true;
}
void endOffboardMode()
{
// bugbug: I removed this releaseControl because it makes back-to-back move operations less smooth.
// The side effect of this is that with some drones (e.g. PX4 based) the drone itself will timeout
// when you stop sending move commands and the behavior on timeout is then determined by the drone itself.
// mav_vehicle_->releaseControl();
ensureSafeMode();
}
void ensureSafeMode()
{
if (mav_vehicle_ != nullptr) {
const mavlinkcom::VehicleState& state = mav_vehicle_->getVehicleState();
if (state.controls.landed || !state.controls.armed) {
return;
}
}
}
void checkValidVehicle()
{
if (mav_vehicle_ == nullptr || connection_ == nullptr || !connection_->isOpen() || !connected_) {
throw std::logic_error("Cannot perform operation when no vehicle is connected or vehicle is not responding");
}
}
//status update methods should call this first!
void updateState() const
{
StatusLock lock(this);
if (mav_vehicle_ != nullptr) {
int version = mav_vehicle_->getVehicleStateVersion();
if (version != state_version_) {
current_state_ = mav_vehicle_->getVehicleState();
state_version_ = version;
}
}
}
virtual void normalizeRotorControls()
{
//if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case
//we normalize them to 0 to 1 for PX4
if (!is_controls_0_1_) {
// change -1 to 1 to 0 to 1.
for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
rotor_controls_[i] = (rotor_controls_[i] + 1.0f) / 2.0f;
}
}
else {
//this applies to PX4 and may work differently on other firmwares.
//We use 0.2 as idle rotors which leaves out range of 0.8
for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
rotor_controls_[i] = Utils::clip(0.8f * rotor_controls_[i] + 0.20f, 0.0f, 1.0f);
}
}
}
bool sendTestMessage(std::shared_ptr<mavlinkcom::MavLinkNode> node)
{
try {
// try and send a test message.
mavlinkcom::MavLinkHeartbeat test;
test.autopilot = static_cast<int>(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4);
test.type = static_cast<uint8_t>(mavlinkcom::MAV_TYPE::MAV_TYPE_GCS);
test.base_mode = 0;
test.custom_mode = 0;
test.mavlink_version = 3;
node->sendMessage(test);
test.system_status = 0;
return true;
}
catch (std::exception&) {
return false;
}
}
bool connectToLogViewer()
{
//set up logviewer proxy
if (connection_info_.logviewer_ip_address.size() > 0) {
std::shared_ptr<mavlinkcom::MavLinkConnection> connection;
createProxy("LogViewer", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_port, connection_info_.local_host_ip, logviewer_proxy_, connection);
if (!sendTestMessage(logviewer_proxy_)) {
// error talking to log viewer, so don't keep trying, and close the connection also.
logviewer_proxy_->getConnection()->close();
logviewer_proxy_ = nullptr;
}
std::shared_ptr<mavlinkcom::MavLinkConnection> out_connection;
createProxy("LogViewerOut", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_sport, connection_info_.local_host_ip, logviewer_out_proxy_, out_connection);
if (!sendTestMessage(logviewer_out_proxy_)) {
// error talking to log viewer, so don't keep trying, and close the connection also.
logviewer_out_proxy_->getConnection()->close();
logviewer_out_proxy_ = nullptr;
}
else if (mav_vehicle_ != nullptr) {
auto proxylog = std::make_shared<MavLinkLogViewerLog>(logviewer_out_proxy_);
mav_vehicle_->getConnection()->startLoggingSendMessage(proxylog);
mav_vehicle_->getConnection()->startLoggingReceiveMessage(proxylog);
if (connection_ != nullptr) {
connection_->startLoggingSendMessage(proxylog);
connection_->startLoggingReceiveMessage(proxylog);
}
}
}
return logviewer_proxy_ != nullptr;
}
bool connectToQGC()
{
if (connection_info_.qgc_ip_address.size() > 0) {
std::shared_ptr<mavlinkcom::MavLinkConnection> connection;
createProxy("QGC", connection_info_.qgc_ip_address, connection_info_.qgc_ip_port, connection_info_.local_host_ip, qgc_proxy_, connection);
if (!sendTestMessage(qgc_proxy_)) {
// error talking to QGC, so don't keep trying, and close the connection also.
qgc_proxy_->getConnection()->close();
qgc_proxy_ = nullptr;
}
else {
connection->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection_val, const mavlinkcom::MavLinkMessage& msg) {
unused(connection_val);
processQgcMessages(msg);
});
}
}
return qgc_proxy_ != nullptr;
}
void createProxy(std::string name, std::string ip, int port, string local_host_ip,
std::shared_ptr<mavlinkcom::MavLinkNode>& node, std::shared_ptr<mavlinkcom::MavLinkConnection>& connection)
{
if (connection_ == nullptr)
throw std::domain_error("MavLinkMultirotorApi requires connection object to be set before createProxy call");
connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port);
// it is ok to reuse the simulator sysid and compid here because this node is only used to send a few messages directly to this endpoint
// and all other messages are funneled through from PX4 via the Join method below.
node = std::make_shared<mavlinkcom::MavLinkNode>(connection_info_.sim_sysid, connection_info_.sim_compid);
node->connect(connection);
// now join the main connection to this one, this causes all PX4 messages to be sent to the proxy and all messages from the proxy will be
// send directly to the PX4 (using whatever sysid/compid comes from that remote node).
connection_->join(connection);
auto mavcon = mav_vehicle_->getConnection();
if (mavcon != connection_) {
mavcon->join(connection);
}
}
static std::string findPX4()
{
auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0);
for (auto iter = result.begin(); iter != result.end(); iter++) {
mavlinkcom::SerialPortInfo info = *iter;
if ((
(info.vid == pixhawkVendorId) &&
(info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId || info.pid == pixhawkFMUV5ProductId)) ||
((info.displayName.find(L"PX4_") != std::string::npos))) {
// printf("Auto Selecting COM port: %S\n", info.displayName.c_str());
std::string portName_str;
for (wchar_t ch : info.portName) {
portName_str.push_back(static_cast<char>(ch));
}
return portName_str;
}
}
return "";
}
void createMavConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info)
{
if (connection_info.use_serial) {
createMavSerialConnection(connection_info.serial_port, connection_info.baud_rate);
}
else {
createMavEthernetConnection(connection_info);
}
//Uncomment below for sending images over MavLink
//connectToVideoServer();
}
void createMavEthernetConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info)
{
close();
connecting_ = true;
is_controls_0_1_ = true;
std::string remoteIpAddr;
Utils::setValue(rotor_controls_, 0.0f);
if (connection_info.use_tcp) {
if (connection_info.tcp_port == 0) {
throw std::invalid_argument("TcpPort setting has an invalid value.");
}
auto msg = Utils::stringf("Waiting for TCP connection on port %d, local IP %s", connection_info.tcp_port, connection_info_.local_host_ip.c_str());
addStatusMessage(msg);
try {
connection_ = std::make_shared<mavlinkcom::MavLinkConnection>();
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?");
addStatusMessage(e.what());
return;
}
}
else if (connection_info.udp_address.size() > 0) {
if (connection_info.udp_port == 0) {
throw std::invalid_argument("UdpPort setting has an invalid value.");
}
connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, connection_info.udp_address, connection_info.udp_port);
}
else {
throw std::invalid_argument("Please provide valid connection info for your drone.");
}
// start listening to the SITL connection.
connection_->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection, const mavlinkcom::MavLinkMessage& msg) {
unused(connection);
processMavMessages(msg);
});
hil_node_ = std::make_shared<mavlinkcom::MavLinkNode>(connection_info_.sim_sysid, connection_info_.sim_compid);
hil_node_->connect(connection_);
if (connection_info.use_tcp) {
addStatusMessage(std::string("Connected to SITL over TCP."));
}
else {
addStatusMessage(std::string("Connected to SITL over UDP."));
}
mav_vehicle_ = std::make_shared<mavlinkcom::MavLinkVehicle>(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
if (connection_info_.control_ip_address != "") {
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 %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 {
std::shared_ptr<mavlinkcom::MavLinkConnection> 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."));
}
else {
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());
}
}
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 != nullptr && mavcon != connection_) {
mavcon->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection, const mavlinkcom::MavLinkMessage& msg) {
unused(connection);
processControlMessages(msg);
});
}
else {
mav_vehicle_->connect(connection_);
}
connected_ = true;
// 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)
{
close();
connecting_ = true;
bool reported = false;
std::string port_name_auto = port_name;
while (port_name_auto == "" || port_name_auto == "*") {
port_name_auto = findPX4();
if (port_name_auto == "") {
if (!reported) {
reported = true;
addStatusMessage("Could not detect a connected PX4 flight controller on any USB ports.");
addStatusMessage("You can specify USB port in settings.json.");
}
std::this_thread::sleep_for(std::chrono::seconds(1));
}
}
if (port_name_auto == "") {
addStatusMessage("USB port for PX4 flight controller is empty. Please set it in settings.json.");
return;
}
if (baud_rate == 0) {
addStatusMessage("Baud rate specified in settings.json is 0 which is invalid");
return;
}
addStatusMessage(Utils::stringf("Connecting to PX4 over serial port: %s, baud rate %d ....", port_name_auto.c_str(), baud_rate));
reported = false;
while (connecting_) {
try {
connection_ = mavlinkcom::MavLinkConnection::connectSerial("hil", port_name_auto, baud_rate);
connection_->ignoreMessage(mavlinkcom::MavLinkAttPosMocap::kMessageId); //TODO: find better way to communicate debug pose instead of using fake Mo-cap messages
hil_node_ = std::make_shared<mavlinkcom::MavLinkNode>(connection_info_.sim_sysid, connection_info_.sim_compid);
hil_node_->connect(connection_);
addStatusMessage(Utils::stringf("Connected to PX4 over serial port: %s", port_name_auto.c_str()));
// start listening to the HITL connection.
connection_->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection, const mavlinkcom::MavLinkMessage& msg) {
unused(connection);
processMavMessages(msg);
});
mav_vehicle_ = std::make_shared<mavlinkcom::MavLinkVehicle>(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
connectVehicle();
return;
}
catch (std::exception& e) {
if (!reported) {
reported = true;
addStatusMessage("Error connecting to mavlink vehicle.");
addStatusMessage(e.what());
addStatusMessage("Please check your USB port in settings.json.");
}
std::this_thread::sleep_for(std::chrono::seconds(1));
}
}
}
mavlinkcom::MavLinkHilSensor getLastSensorMessage()
{
std::lock_guard<std::mutex> guard(last_message_mutex_);
return last_sensor_message_;
}
mavlinkcom::MavLinkDistanceSensor getLastDistanceMessage()
{
std::lock_guard<std::mutex> guard(last_message_mutex_);
return last_distance_message_;
}
mavlinkcom::MavLinkHilGps getLastGpsMessage()
{
std::lock_guard<std::mutex> guard(last_message_mutex_);
return last_gps_message_;
}
void sendParams()
{
// send any mavlink parameters from settings.json through to the connected vehicle.
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
for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
rotor_controls_[i] = 0;
}
}
}
void processQgcMessages(const mavlinkcom::MavLinkMessage& msg)
{
if (msg.msgid == MocapPoseMessage.msgid) {
std::lock_guard<std::mutex> guard(mocap_pose_mutex_);
MocapPoseMessage.decode(msg);
getMocapPose(mocap_pose_.position, mocap_pose_.orientation);
}
//else ignore message
}
void addStatusMessage(const std::string& message)
{
if (message.size() != 0) {
Utils::log(message);
std::lock_guard<std::mutex> guard_status(status_text_mutex_);
//if queue became too large, clear it first
if (status_messages_.size() > status_messages_MaxSize)
Utils::clear(status_messages_, status_messages_MaxSize - status_messages_.size());
status_messages_.push(message);
}
}
void handleLockStep()
{
received_actuator_controls_ = true;
// if the timestamps match then it means we are in lockstep mode.
if (!lock_step_active_ && lock_step_enabled_) {
// && (HilActuatorControlsMessage.flags & 0x1)) // todo: enable this check when this flag is widely available...
if (last_hil_sensor_time_ == HilActuatorControlsMessage.time_usec) {
addStatusMessage("Enabling lockstep mode");
lock_step_active_ = true;
}
}
else {
auto now = clock()->nowNanos() / 1000;
auto delay = static_cast<uint32_t>(now - last_update_time_);
std::lock_guard<std::mutex> guard(telemetry_mutex_);
actuator_delay_ += delay;
}
if (world_ != nullptr) {
world_->pause(false);
}
}
void processMavMessages(const mavlinkcom::MavLinkMessage& msg)
{
if (msg.msgid == HeartbeatMessage.msgid) {
std::lock_guard<std::mutex> guard_heartbeat(heartbeat_mutex_);
HeartbeatMessage.decode(msg);
bool armed = (HeartbeatMessage.base_mode & static_cast<uint8_t>(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) > 0;
setArmed(armed);
if (!got_first_heartbeat_) {
Utils::log("received first heartbeat");
got_first_heartbeat_ = true;
if (HeartbeatMessage.autopilot == static_cast<uint8_t>(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4) &&
HeartbeatMessage.type == static_cast<uint8_t>(mavlinkcom::MAV_TYPE::MAV_TYPE_FIXED_WING)) {
// PX4 will scale fixed wing servo outputs to -1 to 1
// and it scales multi rotor servo output to 0 to 1.
is_controls_0_1_ = false;
}
}
else if (is_simulation_mode_ && !is_hil_mode_set_) {
setHILMode();
}
}
else if (msg.msgid == StatusTextMessage.msgid) {
StatusTextMessage.decode(msg);
//lock is established by below method
addStatusMessage(std::string(StatusTextMessage.text));
}
else if (msg.msgid == CommandLongMessage.msgid) {
CommandLongMessage.decode(msg);
if (CommandLongMessage.command == static_cast<int>(mavlinkcom::MAV_CMD::MAV_CMD_SET_MESSAGE_INTERVAL)) {
int msg_id = static_cast<int>(CommandLongMessage.param1 + 0.5);
if (msg_id == 115) { //HIL_STATE_QUATERNION
hil_state_freq_ = static_cast<int>(CommandLongMessage.param2 + 0.5);
}
}
}
else if (msg.msgid == HilControlsMessage.msgid) {
if (!actuators_message_supported_) {
std::lock_guard<std::mutex> guard_controls(hil_controls_mutex_);
HilControlsMessage.decode(msg);
rotor_controls_[0] = HilControlsMessage.roll_ailerons;
rotor_controls_[1] = HilControlsMessage.pitch_elevator;
rotor_controls_[2] = HilControlsMessage.yaw_rudder;
rotor_controls_[3] = HilControlsMessage.throttle;
rotor_controls_[4] = HilControlsMessage.aux1;
rotor_controls_[5] = HilControlsMessage.aux2;
rotor_controls_[6] = HilControlsMessage.aux3;
rotor_controls_[7] = HilControlsMessage.aux4;
normalizeRotorControls();
handleLockStep();
}
}
else if (msg.msgid == HilActuatorControlsMessage.msgid) {
actuators_message_supported_ = true;
std::lock_guard<std::mutex> guard_actuator(hil_controls_mutex_); //use same mutex as HIL_CONTROl
HilActuatorControlsMessage.decode(msg);
bool isarmed = (HilActuatorControlsMessage.mode & 128) != 0;
for (auto i = 0; i < 8; ++i) {
if (isarmed) {
rotor_controls_[i] = HilActuatorControlsMessage.controls[i];
}
else {
rotor_controls_[i] = 0;
}
}
if (isarmed) {
normalizeRotorControls();
}
handleLockStep();
}
else if (msg.msgid == MavLinkGpsRawInt.msgid) {
MavLinkGpsRawInt.decode(msg);
auto fix_type = static_cast<mavlinkcom::GPS_FIX_TYPE>(MavLinkGpsRawInt.fix_type);
auto locked = (fix_type != mavlinkcom::GPS_FIX_TYPE::GPS_FIX_TYPE_NO_GPS &&
fix_type != mavlinkcom::GPS_FIX_TYPE::GPS_FIX_TYPE_NO_FIX);
if (locked && !has_gps_lock_) {
addStatusMessage("Got GPS lock");
has_gps_lock_ = true;
}
if (!has_home_ && current_state_.home.is_set) {
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.
if (current_state_.controls.landed) {
monitorGroundAltitude();
}
}
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("Ignoring msg %d from %d,%d ", msg.msgid, msg.compid, msg.sysid));
}
}
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");
mavlinkcom::MavLinkHilSensor hil_sensor;
hil_sensor.time_usec = last_hil_sensor_time_ = getSimTime();
hil_sensor.xacc = acceleration.x();
hil_sensor.yacc = acceleration.y();
hil_sensor.zacc = acceleration.z();
hil_sensor.fields_updated = 0b111; // Set accel bit fields
hil_sensor.xgyro = gyro.x();
hil_sensor.ygyro = gyro.y();
hil_sensor.zgyro = gyro.z();
hil_sensor.fields_updated |= 0b111000; // Set gyro bit fields
hil_sensor.xmag = mag.x();
hil_sensor.ymag = mag.y();
hil_sensor.zmag = mag.z();
hil_sensor.fields_updated |= 0b111000000; // Set mag bit fields
hil_sensor.abs_pressure = abs_pressure;
hil_sensor.pressure_alt = pressure_alt;
hil_sensor.fields_updated |= 0b1101000000000; // Set baro bit fields
//TODO: enable temperature? diff_pressure
if (was_reset_) {
hil_sensor.fields_updated = static_cast<uint32_t>(1 << 31);
}
if (hil_node_ != nullptr) {
//auto msg = Utils::stringf("Send HIL Sensor");
//addStatusMessage(msg);
hil_node_->sendMessage(hil_sensor);
received_actuator_controls_ = false;
if (lock_step_active_ && world_ != nullptr) {
world_->pauseForTime(1); // 1 second delay max waiting for actuator controls.
}
}
std::lock_guard<std::mutex> guard(last_message_mutex_);
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,
uint8_t sensor_type, uint8_t sensor_id, const 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<uint32_t>(getSimTime() / 1000);
distance_sensor.min_distance = static_cast<uint16_t>(min_distance);
distance_sensor.max_distance = static_cast<uint16_t>(max_distance);
distance_sensor.current_distance = static_cast<uint16_t>(current_distance);
distance_sensor.type = sensor_type;
distance_sensor.id = sensor_id;
// Use custom orientation
distance_sensor.orientation = 100; // MAV_SENSOR_ROTATION_CUSTOM，自定义角度
distance_sensor.quaternion[0] = orientation.w();
distance_sensor.quaternion[1] = orientation.x();
distance_sensor.quaternion[2] = orientation.y();
distance_sensor.quaternion[3] = orientation.z();
distance_sensor.signal_quality = 100; // 信号最好
//TODO: use covariance parameter?
// PX4 doesn't support receiving simulated distance sensor messages this way.
// It results in lots of error messages from PX4. This code is still useful in that
// it sets last_distance_message_ and that is returned via Python API.
//
if (hil_node_ != nullptr) {
//auto msg = Utils::stringf("Send Distance Sensor, id=%d, distance=%f", sensor_id, current_distance);
//addStatusMessage(msg);
hil_node_->sendMessage(distance_sensor);
}
std::lock_guard<std::mutex> guard(last_message_mutex_);
last_distance_message_ = distance_sensor;
}
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");
mavlinkcom::MavLinkHilGps hil_gps;
hil_gps.time_usec = getSimTime();
hil_gps.lat = static_cast<int32_t>(geo_point.latitude * 1E7);
hil_gps.lon = static_cast<int32_t>(geo_point.longitude * 1E7);
hil_gps.alt = static_cast<int32_t>(geo_point.altitude * 1000);
hil_gps.vn = static_cast<int16_t>(velocity.x() * 100);
hil_gps.ve = static_cast<int16_t>(velocity.y() * 100);
hil_gps.vd = static_cast<int16_t>(velocity.z() * 100);
hil_gps.eph = static_cast<uint16_t>(eph * 100);
hil_gps.epv = static_cast<uint16_t>(epv * 100);
hil_gps.fix_type = static_cast<uint8_t>(fix_type);
hil_gps.vel = static_cast<uint16_t>(velocity_xy * 100);
hil_gps.cog = static_cast<uint16_t>(cog * 100);
hil_gps.satellites_visible = static_cast<uint8_t>(15);
if (hil_node_ != nullptr) {
//auto msg = Utils::stringf("Send HIL GPS");
//addStatusMessage(msg);
hil_node_->sendMessage(hil_gps);
}
if (hil_gps.lat < 0.1f && hil_gps.lat > -0.1f) {
//Utils::DebugBreak();
Utils::log("hil_gps.lat was too close to 0", Utils::kLogLevelError);
}
std::lock_guard<std::mutex> guard(last_message_mutex_);
last_gps_message_ = hil_gps;
}
void resetState()
{
//reset state
is_hil_mode_set_ = false;
is_controls_0_1_ = true;
hil_state_freq_ = -1;
actuators_message_supported_ = false;
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_update_time_ = 0;
last_hil_sensor_time_ = 0;
update_count_ = 0;
hil_sensor_count_ = 0;
lock_step_resets_ = 0;
actuator_delay_ = 0;
is_api_control_enabled_ = false;
thrust_controller_ = PidController();
Utils::setValue(rotor_controls_, 0.0f);
was_reset_ = false;
received_actuator_controls_ = false;
lock_step_active_ = false;
lock_step_enabled_ = false;
has_gps_lock_ = false;
send_params_ = false;
mocap_pose_ = Pose::nanPose();
ground_variance_ = 1;
ground_filter_.initialize(25, 0.1f);
cancelLastTask();
}
void monitorGroundAltitude()
{
// used to ensure stable altitude before takeoff.
auto position = getPosition();
auto result = ground_filter_.filter(position.z());
auto variance = std::get<1>(result);
if (variance >= 0) { // filter returns -1 if we don't have enough data yet.
ground_variance_ = variance;
}
}
protected: //variables
//TODO: below was made protected from private to support Ardupilot
//implementation but we need to review this and avoid having protected variables
static const int RotorControlsCount = 8;
const SensorCollection* sensors_;
mutable std::mutex hil_controls_mutex_;
AirSimSettings::MavLinkConnectionInfo connection_info_;
float rotor_controls_[RotorControlsCount];
bool is_simulation_mode_;
private: //variables
static const int pixhawkVendorId = 9900; ///< Vendor ID for Pixhawk board (V2 and V1) and PX4 Flow
static const int pixhawkFMUV4ProductId = 18; ///< Product ID for Pixhawk V2 board
static const int pixhawkFMUV2ProductId = 17; ///< Product ID for Pixhawk V2 board
static const int pixhawkFMUV5ProductId = 50; ///< Product ID for Pixhawk V5 board
static const int pixhawkFMUV2OldBootloaderProductId = 22; ///< Product ID for Bootloader on older Pixhawk V2 boards
static const int pixhawkFMUV1ProductId = 16; ///< Product ID for PX4 FMU V1 board
static const int messageReceivedTimeout = 10; ///< Seconds
std::shared_ptr<mavlinkcom::MavLinkNode> logviewer_proxy_, logviewer_out_proxy_, qgc_proxy_;
size_t status_messages_MaxSize = 5000;
std::shared_ptr<mavlinkcom::MavLinkNode> hil_node_;
std::shared_ptr<mavlinkcom::MavLinkConnection> connection_;
std::shared_ptr<mavlinkcom::MavLinkVideoServer> video_server_;
std::shared_ptr<MultirotorApiBase> mav_vehicle_control_;
mavlinkcom::MavLinkAttPosMocap MocapPoseMessage;
mavlinkcom::MavLinkHeartbeat HeartbeatMessage;
mavlinkcom::MavLinkSetMode SetModeMessage;
mavlinkcom::MavLinkStatustext StatusTextMessage;
mavlinkcom::MavLinkHilControls HilControlsMessage;
mavlinkcom::MavLinkHilActuatorControls HilActuatorControlsMessage;
mavlinkcom::MavLinkGpsRawInt MavLinkGpsRawInt;
mavlinkcom::MavLinkCommandLong CommandLongMessage;
mavlinkcom::MavLinkLocalPositionNed MavLinkLocalPositionNed;
mavlinkcom::MavLinkHilSensor last_sensor_message_;
mavlinkcom::MavLinkDistanceSensor last_distance_message_;
mavlinkcom::MavLinkHilGps last_gps_message_;
std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, last_message_mutex_, telemetry_mutex_;
//variables required for VehicleApiBase implementation
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<std::string> status_messages_;
int hil_state_freq_;
bool actuators_message_supported_ = false;
bool was_reset_ = false;
bool has_home_ = false;
bool is_ready_ = false;
bool has_gps_lock_ = false;
bool lock_step_active_ = false;
bool lock_step_enabled_ = false;
bool received_actuator_controls_ = false;
std::string is_ready_message_;
Pose mocap_pose_;
std::thread connect_thread_;
bool connecting_ = false;
bool connected_ = false;
common_utils::SmoothingFilter<float> ground_filter_;
double ground_variance_ = 1;
const double GroundTolerance = 0.1;
// variables for throttling HIL_SENSOR and SYSTEM_TIME messages
uint32_t last_sys_time_ = 0;
unsigned long long sim_time_us_ = 0;
uint64_t last_gps_time_ = 0;
uint64_t last_update_time_ = 0;
uint64_t last_hil_sensor_time_ = 0;
// variables accumulated for MavLinkTelemetry messages.
uint64_t update_time_ = 0;
uint32_t update_count_ = 0;
uint32_t hil_sensor_count_ = 0;
uint32_t lock_step_resets_ = 0;
uint32_t actuator_delay_ = 0;
std::thread telemetry_thread_;
//additional variables required for MultirotorApiBase implementation
//this is optional for methods that might not use vehicle commands
std::shared_ptr<mavlinkcom::MavLinkVehicle> mav_vehicle_;
float target_height_;
bool is_api_control_enabled_;
PidController thrust_controller_;
common_utils::Timer hil_message_timer_;
common_utils::Timer gcs_message_timer_;
std::shared_ptr<mavlinkcom::MavLinkFileLog> log_;
std::string log_file_name_;
World* world_;
//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
mutable int state_version_;
mutable mavlinkcom::VehicleState current_state_;
};
}
} //namespace
#endifsettings.json
json
{
"SettingsVersion": 1.2,
"SimMode": "Multirotor",
"ClockType": "SteppableClock",
"Vehicles": {
"PX4": {
"VehicleType": "PX4Multirotor",
"UseSerial": false,
"LockStep": true,
"UseTcp": true,
"TcpPort": 4560,
"ControlPortLocal": 14030,
"ControlPortRemote": 14280,
"ControlIp": "remote",
"LocalHostIp": "192.168.1.5",
"Sensors":{
"Magnetometer": {
"SensorType": 4,
"Enabled": true
},
"Barometer":{
"SensorType": 1,
"Enabled": true,
"PressureFactorSigma": 0.0001825
},
"Imu": {
"SensorType": 2,
"Enabled" : true,
"AngularRandomWalk": 0.3,
"GyroBiasStabilityTau": 500,
"GyroBiasStability": 4.6,
"VelocityRandomWalk": 0.24,
"AccelBiasStabilityTau": 800,
"AccelBiasStability": 36
},
"Gps": {
"SensorType": 3,
"Enabled" : true,
"EphTimeConstant": 0.9,
"EpvTimeConstant": 0.9,
"EphInitial": 25,
"EpvInitial": 25,
"EphFinal": 0.1,
"EpvFinal": 0.1,
"EphMin3d": 3,
"EphMin2d": 4,
"UpdateLatency": 0.2,
"UpdateFrequency": 50,
"StartupDelay": 1
},
"DistanceFrontMid": {
"SensorType": 5,
"Enabled" : true,
"MinDistance": 0.2,
"MaxDistance": 40,
"X": 0.2, "Y": 0, "Z": -0.5,
"Yaw": 0, "Pitch": 0, "Roll": 0,
"DrawDebugPoints": true,
"ExternalController": true
},
"DistanceFrontLeft": {
"SensorType": 5,
"Enabled" : true,
"MinDistance": 0.2,
"MaxDistance": 40,
"X": 0.2, "Y": 0, "Z": -0.5,
"Yaw": -25, "Pitch": 0, "Roll": 0,
"DrawDebugPoints": true,
"ExternalController": true
},
"DistanceFrontRight": {
"SensorType": 5,
"Enabled" : true,
"MinDistance": 0.2,
"MaxDistance": 40,
"X": 0.2, "Y": 0, "Z": -0.5,
"Yaw": 25, "Pitch": 0, "Roll": 0,
"DrawDebugPoints": true,
"ExternalController": true
},
"DistanceFrontUp": {
"SensorType": 5,
"Enabled" : true,
"MinDistance": 0.2,
"MaxDistance": 40,
"X": 0.2, "Y": 0, "Z": -0.5,
"Yaw": 0, "Pitch": 25, "Roll": 0,
"DrawDebugPoints": true,
"ExternalController": true
},
"DistanceFrontDown": {
"SensorType": 5,
"Enabled" : true,
"MinDistance": 0.2,
"MaxDistance": 40,
"X": 0.2, "Y": 0, "Z": -0.5,
"Yaw": 0, "Pitch": -25, "Roll": 0,
"DrawDebugPoints": true,
"ExternalController": true
}
},
"Parameters": {
"NAV_RCL_ACT": 0,
"NAV_DLL_ACT": 0,
"COM_OBS_AVOID": 0
}
}
}
}PX4 配置文件修改
代码修改完成后，进入 PX4 根目录运行如下代码进行编译
bash
make px4_sitl_default none_irisDISTANCE_SENSOR.hpp
hpp
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef DISTANCE_SENSOR_HPP
#define DISTANCE_SENSOR_HPP
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/distance_sensor.h>
// hw-modify 由于MAVLINK-SDK没有解析wxyz的四元数，因此先根据四元数得到方向参数ORIENTATION
// 后续要修改 MAVLINK-SDK 让其支持四元数，还要修改AirSIM让它的距离传感器支持ORIENTATION设置
class MavlinkStreamDistanceSensor : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamDistanceSensor(mavlink); }
static constexpr const char *get_name_static() { return "DISTANCE_SENSOR"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_DISTANCE_SENSOR; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
return _distance_sensor_subs.advertised_count() * (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
}
private:
explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
// hw-modify 根据四元数计算方向
uint8_t quaternion_to_orientation(float w, float x, float y, float z)
{
// 传感器旋转角，决定前方探测四边形的大小。
// 目前有 90,45,25 三种。因为目前只有45°和90°的宏定义，因此25°也用45°的宏表示，后续要改
#define DISTANCE_ANGLE 25
#define FLOAT_EQUE(F1, F2) (std::fabs((double)(F1) - (double)(F2)) < (double)(1e-4f))
if (FLOAT_EQUE(w,1.0) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,0.0) && FLOAT_EQUE(z,0.0))
{
// 指向前方的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_NONE;
//ROTATION_PITCH_180_YAW_90;
}
#if (DISTANCE_ANGLE==25)
else if (FLOAT_EQUE(w,0.97630) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,0.0) && FLOAT_EQUE(z,0.21644))
{
// 指向右前方的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_45;
}
else if (FLOAT_EQUE(w,0.97630) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,0.0) && FLOAT_EQUE(z,(-0.21644)))
{
// 指向左前方的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_135;
}
else if (FLOAT_EQUE(w,0.97630) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,0.21644) && FLOAT_EQUE(z,0.0))
{
// 指向前上方的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_ROLL_90_YAW_135;
}
else if (FLOAT_EQUE(w,0.97630) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,(-0.21644)) && FLOAT_EQUE(z,0.0))
{
// 指向前下方的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_ROLL_90_YAW_45;
}
#elif (DISTANCE_ANGLE==45)
else if (FLOAT_EQUE(w,0.92388) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,0.0) && FLOAT_EQUE(z,0.38268))
{
// 指向右前方的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_45;
}
else if (FLOAT_EQUE(w,0.92388) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,0.0) && FLOAT_EQUE(z,(-0.38268)))
{
// 指向左前方的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_135;
}
else if (FLOAT_EQUE(w,0.92388) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,0.38268) && FLOAT_EQUE(z,0.0))
{
// 指向前上方的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_ROLL_90_YAW_135;
}
else if (FLOAT_EQUE(w,0.92388) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,(-0.38268)) && FLOAT_EQUE(z,0.0))
{
// 指向前下方的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_ROLL_90_YAW_45;
}
#elif (DISTANCE_ANGLE==90)
else if (FLOAT_EQUE(w,0.70711) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,0.0) && FLOAT_EQUE(z,(-0.70711)))
{
// 指向左边的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_270;
}
else if (FLOAT_EQUE(w,0.70711) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,0.0) && FLOAT_EQUE(z,0.70711))
{
// 指向右边的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_90;
}
else if (FLOAT_EQUE(w,0.70711) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,0.70711) && FLOAT_EQUE(z,0.0))
{
// 指向上方的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_90;
}
else if (FLOAT_EQUE(w,0.70711) && FLOAT_EQUE(x,0.0) && FLOAT_EQUE(y,(-0.70711)) && FLOAT_EQUE(z,0.0))
{
// 指向下方的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_270;
}
#endif
else
{
// 不是上述6个方向的传感器
return MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_CUSTOM;
}
}
bool send() override
{
bool updated = false;
for (int i = 0; i < _distance_sensor_subs.size(); i++) {
distance_sensor_s dist_sensor;
if (_distance_sensor_subs[i].update(&dist_sensor)) {
mavlink_distance_sensor_t msg{};
msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */
switch (dist_sensor.type) {
case MAV_DISTANCE_SENSOR_ULTRASOUND:
msg.type = MAV_DISTANCE_SENSOR_ULTRASOUND;
break;
case MAV_DISTANCE_SENSOR_LASER:
msg.type = MAV_DISTANCE_SENSOR_LASER;
break;
case MAV_DISTANCE_SENSOR_INFRARED:
msg.type = MAV_DISTANCE_SENSOR_INFRARED;
break;
default:
msg.type = MAV_DISTANCE_SENSOR_LASER;
break;
}
msg.current_distance = dist_sensor.current_distance * 1e2f; // m to cm
msg.id = i;
msg.max_distance = dist_sensor.max_distance * 1e2f; // m to cm
msg.min_distance = dist_sensor.min_distance * 1e2f; // m to cm
msg.orientation = dist_sensor.orientation;
msg.covariance = dist_sensor.variance * 1e4f; // m^2 to cm^2
// hw-modify 补全信息
msg.orientation = quaternion_to_orientation(dist_sensor.q[0], dist_sensor.q[1], dist_sensor.q[2], dist_sensor.q[3]);
msg.horizontal_fov = dist_sensor.h_fov;
msg.vertical_fov = dist_sensor.v_fov;
msg.quaternion[0] = dist_sensor.q[0];
msg.quaternion[1] = dist_sensor.q[1];
msg.quaternion[2] = dist_sensor.q[2];
msg.quaternion[3] = dist_sensor.q[3];
msg.signal_quality = dist_sensor.signal_quality;
mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg);
updated = true;
}
}
return updated;
}
};
#endif // DISTANCE_SENSORPython运行脚本代码
hw-avoidance-simple.py
python
#!/usr/bin/env python3
import math
import asyncio
from mavsdk import System
from mavsdk import mission_raw
from mavsdk.telemetry import (DistanceSensor, PositionNed)
from mavsdk.offboard import (OffboardError, AccelerationNed, PositionNedYaw, PositionGlobalYaw, VelocityBodyYawspeed)
from mavsdk.mission import (MissionItem, MissionPlan)
glb_leave_home = False # 全局变量 保存是否离开了home，在home附近是不会触发避障逻辑
glb_mission_current = 0 # 全局变量 保存最新进度
glb_mission_total = 0 # 全局变量 保存任务总数
glb_distance_front = 1000.0 # 全局变量 保存前方距离
glb_distance_back = 1000.0 # 全局变量 保存后方距离
glb_distance_left = 1000.0 # 全局变量 保存左前方距离，航向左转45都
glb_distance_right = 1000.0 # 全局变量 保存右前方距离，航向右转45度
glb_distance_up = 1000.0 # 全局变量 保存上前方距离，航向上转45度
glb_distance_down = 1000.0 # 全局变量 保存下前方方距离，航向下转45度
glb_distance_min = 1000.0 # 全局变量 保存所有传感器中的最小距离
# 定期打印所有信息 ################################################################
async def periodically_print():
while True:
print(f"\n\nProgress: {glb_mission_current}/{glb_mission_total}")
print(f"Distances: \n"
f"Front: {glb_distance_front}m \n"
f"Back : {glb_distance_back}m \n"
f"Left : {glb_distance_left}m \n"
f"Right: {glb_distance_right}m \n"
f"Up : {glb_distance_up}m \n"
f"Down : {glb_distance_down}m \n")
await asyncio.sleep(1) # 每秒打印一次
# 监控是否离开了home ##############################################################
async def check_leave_home(drone: System):
global glb_leave_home
# 等待获取Home点坐标
async for home in drone.telemetry.home():
home_lat = home.latitude_deg
home_lon = home.longitude_deg
break
while True:
# 获取当前位置坐标
async for position in drone.telemetry.position():
current_lat = position.latitude_deg
current_lon = position.longitude_deg
break
#print(f"Home坐标: ({home_lat:.9f}, {home_lon:.9f})")
#print(f"当前坐标: ({current_lat:.9f}, {current_lon:.9f})\n\n")
# 判断是否在home附近
if (abs(current_lat-home_lat) + abs(current_lon-home_lon)) > 0.000015:
#print("离开home")
glb_leave_home = True
else:
#print("在home附近")
glb_leave_home = False
await asyncio.sleep(0.03)
# 监控任务进度 #################################################################
async def monitor_progress(drone: System):
global glb_mission_current
global glb_mission_total
async for mission_progress in drone.mission_raw.mission_progress():
glb_mission_current = mission_progress.current
glb_mission_total = mission_progress.total
print(f"Update Mission progress: "
f"{glb_mission_current}/"
f"{glb_mission_total}")
# 监控距离传感器 #############################################################
async def monitor_distance(drone: System):
global glb_distance_front
global glb_distance_left
global glb_distance_right
global glb_distance_up
global glb_distance_down
global glb_distance_min
# 获取所有距离传感器信息
async for distance_sensor in drone.telemetry.distance_sensor():
# 提取传感器姿态角（单位：度）
roll = distance_sensor.orientation.roll_deg
pitch = distance_sensor.orientation.pitch_deg
yaw = distance_sensor.orientation.yaw_deg
current_distance = distance_sensor.current_distance_m
# 根据姿态角判断方向，后续优化为初始化时判断一下，判断通过就不再监控
if math.isclose(yaw, 0.0) and math.isclose(pitch, 0.0) and math.isclose(roll, 0.0):
# 前
glb_distance_front = current_distance
elif math.isclose(yaw, 45.0) and math.isclose(pitch, 0.0) and math.isclose(roll, 0.0):
# 右
glb_distance_right = current_distance
elif math.isclose(yaw, 135.0) and math.isclose(pitch, 0.0) and math.isclose(roll, 0.0):
# 左
glb_distance_left = current_distance
elif math.isclose(yaw, 45.0) and math.isclose(pitch, 0.0) and math.isclose(roll, 90.0):
# 下
glb_distance_down = current_distance
elif math.isclose(yaw, 135.0) and math.isclose(pitch, 0.0) and math.isclose(roll, 90.0):
# 上
glb_distance_up = current_distance
else:
print("未知距离他传感器方位\n")
# 获取所有传感器中最小的值
if glb_distance_front < glb_distance_left:
tmp_distance_min = glb_distance_front
else:
tmp_distance_min = glb_distance_left
if tmp_distance_min > glb_distance_right:
tmp_distance_min = glb_distance_right
if tmp_distance_min > glb_distance_up:
tmp_distance_min = glb_distance_up
if tmp_distance_min > glb_distance_down:
tmp_distance_min = glb_distance_down
glb_distance_min = tmp_distance_min
#print(f"最小距离传感器: {glb_distance_min}")
# 简单避障逻辑，遇到障碍上升 #############################################################
AVOIDANCE_TRIGGER_DISTANCE = 7.0 # 避障触发距离阈值
AVOIDANCE_RELEASE_DISTANCE = 15.0 # 避障解除距离阈值
# 方法1：速度避障逻辑，遇到障碍物进行爬升，直到和障碍物的距离大于阈值 ------------------------
async def speed_avoidance(drone: System):
# 适当后退远离障碍物，避免抵住障碍物导致无法爬升
print("--加载速度避障逻辑")
await drone.offboard.set_velocity_body(VelocityBodyYawspeed(0.0, 0.0, 0.0, 0.0))
await drone.offboard.start()
print("--适当倒退")
await drone.offboard.set_velocity_body(
VelocityBodyYawspeed(
-0.1, # X轴速度（前后方向）
0.0, # Y轴速度（左右方向）
0.0, # Z轴速度（上下方向）
0.0 # Yaw角速度（机体旋转绕Z轴旋转，0表示保持航向不变）
)
)
#await drone.offboard.set_acceleration_ned(AccelerationNed(0.000000, 0.000000, -1.0))
await asyncio.sleep(0.5) # 让无人机后退0.5秒，远离障碍物
# 设置爬升加速度
print("--稳定姿态")
await drone.offboard.set_velocity_body(VelocityBodyYawspeed(0.0, 0.0, 0.0, 0.0))
await asyncio.sleep(2)
print("--开始爬升")
await drone.offboard.set_velocity_body(VelocityBodyYawspeed(0.0, 0.0, -1.7, 0.0))
while True:
# 判断前方距离
if glb_distance_min > AVOIDANCE_RELEASE_DISTANCE :
break # 如果前方距离大于阈值，则停止持续爬升逻辑
await asyncio.sleep(1)
await asyncio.sleep(1.7) # 继续爬升1.7秒，留出足够余量
print("--障碍物已远离，停止爬升!")
# 停止Offboard模式，继续执行任务
print("--退出避障逻辑")
print("恢复执行任务...")
await drone.offboard.stop()
await drone.mission.start_mission()
# 避障主循环 ------------------------
async def obstacle_avoidance(drone: System):
while True:
#print(f"chcek{glb_distance_front}")
if glb_leave_home and glb_distance_min < AVOIDANCE_TRIGGER_DISTANCE :
print(f"近距离发现障碍物{glb_distance_min:.2f}，停止前进!")
await drone.mission.pause_mission()
#await asyncio.sleep(1)
await speed_avoidance(drone) # 调用速度避障逻辑
await asyncio.sleep(0.04) # 每0.1秒检查一次
# 主函数 ######################################################################
async def run():
# 连接到飞行器
drone = System()
await drone.connect(system_address="udp://:14540") # 在14540端口上等待PX4的连接
print("等待连接...")
async for state in drone.core.connection_state():
if state.is_connected:
print("已连接!\n")
break
# 启动所有任务
task_check_leave_home = asyncio.create_task(check_leave_home(drone))
#task_monitor_progress = asyncio.create_task(monitor_progress(drone))
task_monitor_distance = asyncio.create_task(monitor_distance(drone))
task_obstacle_avoidance = asyncio.create_task(obstacle_avoidance(drone))
#task_periodically_print = asyncio.create_task(periodically_print())
await asyncio.gather(task_check_leave_home, task_monitor_distance, task_obstacle_avoidance)
# 运行主函数
if __name__ == "__main__":
asyncio.run(run())