Skip to content

Add support for Distance Sensor in ArduPilot #2920

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
rajat2004 wants to merge 4 commits into from
Closed

Add support for Distance Sensor in ArduPilot #2920

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
999aea5
Add support for Distance Sensor in ArduPilot
rajat2004 Aug 7, 2020
d86674a
Add support for Distance Sensor in ArduPilot Rover vehicle
rajat2004 Aug 7, 2020
fddbc5a
Fix style inconsistency, use unique_ptr in ArduPilot files
rajat2004 Aug 31, 2020
a6c62f7
[Unity] Update CMake to C++14 standard
rajat2004 Aug 31, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
47 changes: 37 additions & 10 deletions AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -122,8 +122,8 @@ class ArduRoverApi : public CarApiBase {
protected:
void closeConnections()
{
if (udpSocket_ != nullptr)
udpSocket_->close();
if (udp_socket_ != nullptr)
udp_socket_->close();
}

void connect()
Expand All @@ -144,24 +144,24 @@ class ArduRoverApi : public CarApiBase {
Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);

udpSocket_ = std::make_shared<mavlinkcom::UdpSocket>();
udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
udp_socket_ = std::make_unique<mavlinkcom::UdpSocket>();
udp_socket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
}

private:
void recvRoverControl()
{
// Receive motor data
RoverControlMessage pkt;
int recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100);
int recv_ret = udp_socket_->recv(&pkt, sizeof(pkt), 100);
while (recv_ret != sizeof(pkt)) {
if (recv_ret <= 0) {
Utils::log(Utils::stringf("Error while receiving rotor control data - ErrorNo: %d", recv_ret), Utils::kLogLevelInfo);
} else {
Utils::log(Utils::stringf("Received %d bytes instead of %zu bytes", recv_ret, sizeof(pkt)), Utils::kLogLevelInfo);
}

recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100);
recv_ret = udp_socket_->recv(&pkt, sizeof(pkt), 100);
}

last_controls_.throttle = pkt.throttle;
Expand All @@ -186,7 +186,34 @@ class ArduRoverApi : public CarApiBase {

std::ostringstream oss;

const uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
// Send Distance Sensors data if present
const uint count_distance_sensors = sensors_->size(SensorBase::SensorType::Distance);
if (count_distance_sensors != 0) {
// Start JSON element
oss << ","
"\"rng\": {"
"\"distances\": [";

// Add the first sensor separately since otherwise there's an extra ","
// Which leads to AP counting an extra sensor, and if there are 10 sensors (AP supports upto 10), last one will get ignored
auto distance_output = getDistanceSensorData("");
oss << distance_output.distance;

// Add remaining sensors in the array
for (uint i=1; i<count_distance_sensors; ++i) {
const auto* distance_sensor = static_cast<const DistanceBase*>(sensors_->getByType(SensorBase::SensorType::Distance, i));
if (distance_sensor != nullptr) {
distance_output = distance_sensor->getOutput();
// AP uses meters so no need to convert here
oss << "," << distance_output.distance;
}
}

// Close JSON array & element
oss << "]}";
}

const uint count_lidars = sensors_->size(SensorBase::SensorType::Lidar);
// TODO: Add bool value in settings to check whether to send lidar data or not
// Since it's possible that we don't want to send the lidar data to Ardupilot but still have the lidar (maybe as a ROS topic)
if (count_lidars != 0) {
Expand Down Expand Up @@ -256,8 +283,8 @@ class ArduRoverApi : public CarApiBase {
}

// Send data
if (udpSocket_ != nullptr) {
udpSocket_->sendto(buf, strlen(buf), ip_, port_);
if (udp_socket_ != nullptr) {
udp_socket_->sendto(buf, strlen(buf), ip_, port_);
}
}

Expand All @@ -269,7 +296,7 @@ class ArduRoverApi : public CarApiBase {

AirSimSettings::MavLinkConnectionInfo connection_info_;

std::shared_ptr<mavlinkcom::UdpSocket> udpSocket_;
std::unique_ptr<mavlinkcom::UdpSocket> udp_socket_;

uint16_t port_;
std::string ip_;
Expand Down
62 changes: 46 additions & 16 deletions AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -299,8 +299,8 @@ class ArduCopterApi : public MultirotorApiBase {
protected:
void closeConnections()
{
if (udpSocket_ != nullptr)
udpSocket_->close();
if (udp_socket_ != nullptr)
udp_socket_->close();
}

void connect()
Expand All @@ -320,8 +320,8 @@ class ArduCopterApi : public MultirotorApiBase {
Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);

udpSocket_ = std::make_shared<mavlinkcom::UdpSocket>();
udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
udp_socket_ = std::make_unique<mavlinkcom::UdpSocket>();
udp_socket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
}

private:
Expand Down Expand Up @@ -351,15 +351,45 @@ class ArduCopterApi : public MultirotorApiBase {
<< (last_rcData_.roll + 1) * 0.5f << ","
<< (last_rcData_.yaw + 1) * 0.5f << ","
<< (last_rcData_.throttle + 1) * 0.5f << ","
<< (-last_rcData_.pitch + 1) * 0.5f << ","
<< static_cast<float>(last_rcData_.getSwitch(0)) << ","
<< static_cast<float>(last_rcData_.getSwitch(1)) << ","
<< static_cast<float>(last_rcData_.getSwitch(2)) << ","
<< static_cast<float>(last_rcData_.getSwitch(3))
<< "]}";
<< (-last_rcData_.pitch + 1) * 0.5f;

// Add switches to RC channels array, 8 switches
for (uint8_t i=0; i<8; ++i) {
oss << "," << static_cast<float>(last_rcData_.getSwitch(i));
}

// Close JSON array & element
oss << "]}";
}

// Send Distance Sensors data if present
const uint count_distance_sensors = sensors_->size(SensorBase::SensorType::Distance);
if (count_distance_sensors != 0) {
// Start JSON element
oss << ","
"\"rng\": {"
"\"distances\": [";

// Add the first sensor separately since otherwise there's an extra ","
// Which leads to AP counting an extra sensor, and if there are 10 sensors (AP supports upto 10), last one will get ignored
auto distance_output = getDistanceSensorData("");
oss << distance_output.distance;

// Add remaining sensors in the array
for (uint i=1; i<count_distance_sensors; ++i) {
const auto* distance_sensor = static_cast<const DistanceBase*>(sensors_->getByType(SensorBase::SensorType::Distance, i));
if (distance_sensor != nullptr) {
distance_output = distance_sensor->getOutput();
// AP uses meters so no need to convert here
oss << "," << distance_output.distance;
}
}

// Close JSON array & element
oss << "]}";
}

const uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
const uint count_lidars = sensors_->size(SensorBase::SensorType::Lidar);
// TODO: Add bool value in settings to check whether to send lidar data or not
// Since it's possible that we don't want to send the lidar data to Ardupilot but still have the lidar (maybe as a ROS topic)
if (count_lidars != 0) {
Expand Down Expand Up @@ -429,24 +459,24 @@ class ArduCopterApi : public MultirotorApiBase {
}

// Send data
if (udpSocket_ != nullptr) {
udpSocket_->sendto(buf, strlen(buf), ip_, port_);
if (udp_socket_ != nullptr) {
udp_socket_->sendto(buf, strlen(buf), ip_, port_);
}
}

void recvRotorControl()
{
// Receive motor data
RotorControlMessage pkt;
int recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100);
int recv_ret = udp_socket_->recv(&pkt, sizeof(pkt), 100);
while (recv_ret != sizeof(pkt)) {
if (recv_ret <= 0) {
Utils::log(Utils::stringf("Error while receiving rotor control data - ErrorNo: %d", recv_ret), Utils::kLogLevelInfo);
} else {
Utils::log(Utils::stringf("Received %d bytes instead of %zu bytes", recv_ret, sizeof(pkt)), Utils::kLogLevelInfo);
}

recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100);
recv_ret = udp_socket_->recv(&pkt, sizeof(pkt), 100);
}

for (auto i = 0; i < kArduCopterRotorControlCount; ++i) {
Expand All @@ -463,7 +493,7 @@ class ArduCopterApi : public MultirotorApiBase {
uint16_t pwm[kArduCopterRotorControlCount];
};

std::shared_ptr<mavlinkcom::UdpSocket> udpSocket_;
std::unique_ptr<mavlinkcom::UdpSocket> udp_socket_;

AirSimSettings::MavLinkConnectionInfo connection_info_;
uint16_t port_;
Expand Down
2 changes: 1 addition & 1 deletion Unity/AirLibWrapper/AirsimWrapper/cmake/rpc-setup.cmake
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ set(RPCLIB_DEFAULT_PORT 8080
CACHE STRING "Default port used for running tests and examples")
set(RPCLIB_DEFAULT_BUFFER_SIZE "1024 << 10"
CACHE STRING "Default buffer size")
set(RPCLIB_CXX_STANDARD 11 CACHE STRING
set(RPCLIB_CXX_STANDARD 14 CACHE STRING
"C++ version used to build rpclib (Currently: Only 11 and 14 supported)")

if(RPCLIB_GENERATE_COMPDB)
Expand Down