Skip to content

Commit

Permalink
Merge pull request #2226 from mavlink/pr-autopilot-server-home
Browse files Browse the repository at this point in the history
Autopilot server home fixes
  • Loading branch information
julianoes authored Feb 20, 2024
2 parents afc6cec + eff8047 commit f5e6945
Show file tree
Hide file tree
Showing 6 changed files with 120 additions and 64 deletions.
24 changes: 13 additions & 11 deletions examples/autopilot_server/autopilot_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ Mission::MissionItem make_mission_item(

int main(int argc, char** argv)
{
// We run the server plugins on a seperate thead so we can use the main
// We run the server plugins on a seperate thread so we can use the main
// thread as a ground station.
std::thread autopilotThread([]() {
mavsdk::Mavsdk mavsdkTester{
Expand Down Expand Up @@ -87,6 +87,18 @@ int main(int argc, char** argv)
actionServer.set_allow_takeoff(true);
actionServer.set_armable(true, true);

// Create vehicle telemetry info
TelemetryServer::Position position{55.953251, -3.188267, 0, 0};
TelemetryServer::PositionVelocityNed positionVelocityNed{{0, 0, 0}, {0, 0, 0}};
TelemetryServer::VelocityNed velocity{};
TelemetryServer::Heading heading{60};
TelemetryServer::RawGps rawGps{
0, 55.953251, -3.188267, 0, NAN, NAN, 0, NAN, 0, 0, 0, 0, 0, 0};
TelemetryServer::GpsInfo gpsInfo{11, TelemetryServer::FixType::Fix3D};

// Publish home already, so that it is available.
telemServer.publish_home(position);

// Create a mission raw server
// This will allow us to receive missions from a GCS
auto missionRawServer = mavsdk::MissionRawServer{server_component};
Expand Down Expand Up @@ -114,15 +126,6 @@ int main(int argc, char** argv)
// Set current item to complete to progress the current item state
missionRawServer.set_current_item_complete();

// Create vehicle telemetry info
TelemetryServer::Position position{55.953251, -3.188267, 0, 0};
TelemetryServer::PositionVelocityNed positionVelocityNed{{0, 0, 0}, {0, 0, 0}};
TelemetryServer::VelocityNed velocity{};
TelemetryServer::Heading heading{60};
TelemetryServer::RawGps rawGps{
0, 55.953251, -3.188267, 0, NAN, NAN, 0, NAN, 0, 0, 0, 0, 0, 0};
TelemetryServer::GpsInfo gpsInfo{11, TelemetryServer::FixType::Fix3D};

// As we're acting as an autopilot, lets just make the vehicle jump to 10m altitude on
// successful takeoff
actionServer.subscribe_takeoff([&position](ActionServer::Result result, bool takeoff) {
Expand All @@ -136,7 +139,6 @@ int main(int argc, char** argv)

// Publish the telemetry
telemServer.publish_position(position, velocity, heading);
telemServer.publish_home(position);
telemServer.publish_position_velocity_ned(positionVelocityNed);
telemServer.publish_raw_gps(rawGps, gpsInfo);
}
Expand Down
32 changes: 32 additions & 0 deletions src/mavsdk/core/mavlink_command_receiver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,13 @@ MavlinkCommandReceiver::MavlinkCommandReceiver(ServerComponentImpl& server_compo
MAVLINK_MSG_ID_COMMAND_INT,
[this](const mavlink_message_t& message) { receive_command_int(message); },
this);

if (const char* env_p = std::getenv("MAVSDK_COMMAND_DEBUGGING")) {
if (std::string(env_p) == "1") {
LogDebug() << "Command debugging is on.";
_debugging = true;
}
}
}

MavlinkCommandReceiver::~MavlinkCommandReceiver()
Expand All @@ -32,10 +39,18 @@ void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& messag
{
MavlinkCommandReceiver::CommandInt cmd(message);

if (_debugging) {
LogDebug() << "Received command int " << (int)cmd.command;
}

std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);

for (auto& handler : _mavlink_command_int_handler_table) {
if (handler.cmd_id == cmd.command) {
if (_debugging) {
LogDebug() << "Handling command int " << (int)cmd.command;
}

// The client side can pack a COMMAND_ACK as a response to receiving the command.
auto maybe_command_ack = handler.callback(cmd);
if (maybe_command_ack) {
Expand All @@ -50,6 +65,11 @@ void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& messag
&maybe_command_ack.value());
return response_message;
});

if (_debugging) {
LogDebug() << "Acked command int " << (int)cmd.command << " with "
<< maybe_command_ack.value().result;
}
}
}
}
Expand All @@ -59,10 +79,18 @@ void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& messa
{
MavlinkCommandReceiver::CommandLong cmd(message);

if (_debugging) {
LogDebug() << "Received command long " << (int)cmd.command;
}

std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);

for (auto& handler : _mavlink_command_long_handler_table) {
if (handler.cmd_id == cmd.command) {
if (_debugging) {
LogDebug() << "Handling command long " << (int)cmd.command;
}

// The client side can pack a COMMAND_ACK as a response to receiving the command.
auto maybe_command_ack = handler.callback(cmd);
if (maybe_command_ack) {
Expand All @@ -77,6 +105,10 @@ void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& messa
&maybe_command_ack.value());
return response_message;
});
if (_debugging) {
LogDebug() << "Acked command long " << (int)cmd.command << " with "
<< maybe_command_ack.value().result;
}
}
}
}
Expand Down
2 changes: 2 additions & 0 deletions src/mavsdk/core/mavlink_command_receiver.h
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,8 @@ class MavlinkCommandReceiver {
std::mutex _mavlink_command_handler_table_mutex{};
std::vector<MAVLinkCommandIntHandlerTableEntry> _mavlink_command_int_handler_table{};
std::vector<MAVLinkCommandLongHandlerTableEntry> _mavlink_command_long_handler_table{};

bool _debugging{false};
};

} // namespace mavsdk
3 changes: 3 additions & 0 deletions src/mavsdk/core/mavlink_command_sender.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -192,6 +192,9 @@ void MavlinkCommandSender::receive_command_ack(mavlink_message_t message)
case MAV_RESULT_DENIED:
if (_command_debugging) {
LogDebug() << "command denied (" << work->identification.command << ").";
if (work->identification.command == 512) {
LogDebug() << "(message " << work->identification.maybe_param1 << ")";
}
}
_system_impl.unregister_timeout_handler(work->timeout_cookie);
temp_result = {Result::Denied, NAN};
Expand Down
115 changes: 65 additions & 50 deletions src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,6 @@

namespace mavsdk {

// TODO: fix message cache

TelemetryServerImpl::TelemetryServerImpl(std::shared_ptr<ServerComponent> server_component) :
ServerPluginImplBase(server_component)
{
Expand All @@ -16,7 +14,7 @@ TelemetryServerImpl::TelemetryServerImpl(std::shared_ptr<ServerComponent> server
TelemetryServerImpl::~TelemetryServerImpl()
{
_server_component_impl->unregister_plugin(this);
std::unique_lock<std::mutex> lock(_interval_mutex);
std::lock_guard<std::mutex> lock(_mutex);
for (const auto& request : _interval_requests) {
_server_component_impl->remove_call_every(request.cookie);
}
Expand All @@ -28,7 +26,7 @@ void TelemetryServerImpl::init()
_server_component_impl->register_mavlink_command_handler(
MAV_CMD_SET_MESSAGE_INTERVAL,
[this](const MavlinkCommandReceiver::CommandLong& command) {
std::lock_guard<std::mutex> lock(_interval_mutex);
std::lock_guard<std::mutex> lock(_mutex);
uint32_t msgid = static_cast<uint32_t>(command.params.param1);
// Set interval to 1hz if 0 (default rate)
uint32_t interval_ms =
Expand All @@ -47,7 +45,7 @@ void TelemetryServerImpl::init()
_interval_requests.push_back({msgid, interval_ms, nullptr});
//_server_component_impl->add_call_every(
// [this, msgid]() {
// std::lock_guard<std::mutex> lock_interval(_interval_mutex);
// std::lock_guard<std::mutex> lock_interval(_mutex);
// if (_msg_cache.find(msgid) != _msg_cache.end()) {
// // Publish if callback exists :)
// _server_component_impl->send_message(_msg_cache.at(msgid));
Expand All @@ -72,6 +70,35 @@ void TelemetryServerImpl::init()
command, MAV_RESULT::MAV_RESULT_ACCEPTED);
},
this);

// Handle REQUEST_MESSAGE
_server_component_impl->register_mavlink_command_handler(
MAV_CMD_REQUEST_MESSAGE,
[this](const MavlinkCommandReceiver::CommandLong& command) {
std::lock_guard<std::mutex> lock(_mutex);
uint32_t msgid = static_cast<uint32_t>(command.params.param1);

switch (msgid) {
case MAVLINK_MSG_ID_HOME_POSITION:
if (_maybe_home) {
if (_send_home()) {
return _server_component_impl->make_command_ack_message(
command, MAV_RESULT::MAV_RESULT_ACCEPTED);
} else {
return _server_component_impl->make_command_ack_message(
command, MAV_RESULT::MAV_RESULT_FAILED);
}
} else {
return _server_component_impl->make_command_ack_message(
command, MAV_RESULT::MAV_RESULT_DENIED);
}

default:
return _server_component_impl->make_command_ack_message(
command, MAV_RESULT::MAV_RESULT_DENIED);
}
},
this);
}

void TelemetryServerImpl::deinit() {}
Expand All @@ -81,8 +108,6 @@ TelemetryServer::Result TelemetryServerImpl::publish_position(
TelemetryServer::VelocityNed velocity_ned,
TelemetryServer::Heading heading)
{
// add_msg_cache(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, msg);

return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,
uint8_t channel) {
mavlink_message_t message;
Expand All @@ -108,41 +133,47 @@ TelemetryServer::Result TelemetryServerImpl::publish_position(

TelemetryServer::Result TelemetryServerImpl::publish_home(TelemetryServer::Position home)
{
const float q[4] = {};
_maybe_home = home;

// add_msg_cache(MAVLINK_MSG_ID_HOME_POSITION, msg);
return _send_home() ? TelemetryServer::Result::Success :
TelemetryServer::Result::ConnectionError;
}

bool TelemetryServerImpl::_send_home()
{
// Requires _maybe_home to be set.

const auto home = _maybe_home.value();

const float q[4] = {};

return _server_component_impl->queue_message(
[&](MavlinkAddress mavlink_address, uint8_t channel) {
mavlink_message_t message;
mavlink_msg_home_position_pack_chan(
mavlink_address.system_id,
mavlink_address.component_id,
channel,
&message,
static_cast<int32_t>(home.latitude_deg * 1E7),
static_cast<int32_t>(home.longitude_deg * 1E7),
static_cast<int32_t>(static_cast<double>(home.absolute_altitude_m) * 1E-3),
0, // Local X
0, // Local Y
0, // Local Z
q, // surface normal transform
NAN, // approach x
NAN, // approach y
NAN, // approach z
get_boot_time_ms() // TO-DO: System boot
);
return message;
}) ?
TelemetryServer::Result::Success :
TelemetryServer::Result::Unsupported;
[&](MavlinkAddress mavlink_address, uint8_t channel) {
mavlink_message_t message;
mavlink_msg_home_position_pack_chan(
mavlink_address.system_id,
mavlink_address.component_id,
channel,
&message,
static_cast<int32_t>(home.latitude_deg * 1E7),
static_cast<int32_t>(home.longitude_deg * 1E7),
static_cast<int32_t>(static_cast<double>(home.absolute_altitude_m) * 1E-3),
0, // Local X
0, // Local Y
0, // Local Z
q, // surface normal transform
NAN, // approach x
NAN, // approach y
NAN, // approach z
get_boot_time_ms() // TO-DO: System boot
);
return message;
});
}

TelemetryServer::Result TelemetryServerImpl::publish_raw_gps(
TelemetryServer::RawGps raw_gps, TelemetryServer::GpsInfo gps_info)
{
// add_msg_cache(MAVLINK_MSG_ID_GPS_RAW_INT, msg);

return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,
uint8_t channel) {
mavlink_message_t message;
Expand Down Expand Up @@ -179,8 +210,6 @@ TelemetryServer::Result TelemetryServerImpl::publish_battery(TelemetryServer::Ba
uint16_t voltages_ext[4] = {0};
voltages[0] = static_cast<uint16_t>(static_cast<double>(battery.voltage_v) * 1E3);

// add_msg_cache(MAVLINK_MSG_ID_BATTERY_STATUS, msg);

return _server_component_impl->queue_message(
[&](MavlinkAddress mavlink_address, uint8_t channel) {
mavlink_message_t message;
Expand Down Expand Up @@ -214,8 +243,6 @@ TelemetryServerImpl::publish_distance_sensor(TelemetryServer::DistanceSensor dis
{
std::array<float, 4> q{0}; // Invalid Quaternion per mavlink spec

// add_msg_cache(MAVLINK_MSG_ID_DISTANCE_SENSOR, msg);

return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,
uint8_t channel) {
mavlink_message_t message;
Expand Down Expand Up @@ -309,8 +336,6 @@ TelemetryServer::Result TelemetryServerImpl::publish_odometry(TelemetryServer::O
TelemetryServer::Result TelemetryServerImpl::publish_position_velocity_ned(
TelemetryServer::PositionVelocityNed position_velocity_ned)
{
// add_msg_cache(MAVLINK_MSG_ID_LOCAL_POSITION_NED, msg);

return _server_component_impl->queue_message(
[&](MavlinkAddress mavlink_address, uint8_t channel) {
mavlink_message_t message;
Expand Down Expand Up @@ -399,8 +424,6 @@ TelemetryServer::Result TelemetryServerImpl::publish_sys_status(
sensors |= MAV_SYS_STATUS_SENSOR_GPS;
}

// add_msg_cache(MAVLINK_MSG_ID_SYS_STATUS, msg);

return _server_component_impl->queue_message(
[&](MavlinkAddress mavlink_address, uint8_t channel) {
mavlink_message_t message;
Expand Down Expand Up @@ -468,8 +491,6 @@ uint8_t to_mav_landed_state(TelemetryServer::LandedState landed_state)
TelemetryServer::Result TelemetryServerImpl::publish_extended_sys_state(
TelemetryServer::VtolState vtol_state, TelemetryServer::LandedState landed_state)
{
// add_msg_cache(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, msg);

return _server_component_impl->queue_message(
[&](MavlinkAddress mavlink_address, uint8_t channel) {
mavlink_message_t message;
Expand All @@ -486,10 +507,4 @@ TelemetryServer::Result TelemetryServerImpl::publish_extended_sys_state(
TelemetryServer::Result::Unsupported;
}

void TelemetryServerImpl::add_msg_cache(uint64_t id, mavlink_message_t& msg)
{
std::unique_lock<std::mutex> lock(_interval_mutex);
_msg_cache.insert_or_assign(id, msg);
}

} // namespace mavsdk
8 changes: 5 additions & 3 deletions src/mavsdk/plugins/telemetry_server/telemetry_server_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

#include <unordered_map>
#include <chrono>
#include <optional>

namespace mavsdk {

Expand Down Expand Up @@ -76,13 +77,14 @@ class TelemetryServerImpl : public ServerPluginImplBase {
TelemetryServer::VtolState vtol_state, TelemetryServer::LandedState landed_state);

private:
bool _send_home();

std::chrono::time_point<std::chrono::steady_clock> _start_time;

std::mutex _mutex;
std::vector<RequestMsgInterval> _interval_requests;
std::mutex _interval_mutex;
std::unordered_map<uint64_t, mavlink_message_t> _msg_cache;

void add_msg_cache(uint64_t id, mavlink_message_t& msg);
std::optional<TelemetryServer::Position> _maybe_home{};

uint64_t get_boot_time_ms()
{
Expand Down

0 comments on commit f5e6945

Please sign in to comment.