Skip to content
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

Autopilot server home fixes #2226

Merged
merged 2 commits into from
Feb 20, 2024
Merged

Autopilot server home fixes #2226

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
144768e
core: more debugging info for MAVLink commands
julianoes Feb 19, 2024
eff8047
telemetry_server: provide home position properly
julianoes Feb 19, 2024
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
24 changes: 13 additions & 11 deletions examples/autopilot_server/autopilot_server.cpp
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
Loading