Skip to content

Commit

Permalink
Fixes after rebase
Browse files Browse the repository at this point in the history
  • Loading branch information
julianoes committed Jun 22, 2022
1 parent f69849f commit 2ed0b7a
Show file tree
Hide file tree
Showing 10 changed files with 24 additions and 120 deletions.
4 changes: 2 additions & 2 deletions src/mavsdk/core/flight_mode.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,9 @@
namespace mavsdk {

FlightMode to_flight_mode_from_custom_mode(
Sender::Autopilot autopilot, MAV_TYPE mav_type, uint32_t custom_mode)
System::CompatibilityMode compatibility_mode, MAV_TYPE mav_type, uint32_t custom_mode)
{
if (autopilot == Sender::Autopilot::ArduPilot) {
if (compatibility_mode == System::CompatibilityMode::Ardupilot) {
switch (mav_type) {
case MAV_TYPE::MAV_TYPE_SURFACE_BOAT:
case MAV_TYPE::MAV_TYPE_GROUND_ROVER:
Expand Down
2 changes: 1 addition & 1 deletion src/mavsdk/core/flight_mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ enum class FlightMode {
};

FlightMode to_flight_mode_from_custom_mode(
Sender::Autopilot autopilot, MAV_TYPE mav_type, uint32_t custom_mode);
System::CompatibilityMode compatibility_mode, MAV_TYPE mav_type, uint32_t custom_mode);
FlightMode to_flight_mode_from_px4_mode(uint32_t custom_mode);
FlightMode to_flight_mode_from_ardupilot_rover_mode(uint32_t custom_mode);
FlightMode to_flight_mode_from_ardupilot_copter_mode(uint32_t custom_mode);
Expand Down
2 changes: 1 addition & 1 deletion src/mavsdk/core/mavlink_mission_transfer.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class Sender {
[[nodiscard]] virtual System::CompatibilityMode compatibility_mode() const = 0;
};

class MAVLinkMissionTransfer {
class MavlinkMissionTransfer {
public:
enum class Result {
Success,
Expand Down
3 changes: 2 additions & 1 deletion src/mavsdk/core/mavlink_mission_transfer_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,8 @@ class MavlinkMissionTransferTest : public ::testing::Test {
ON_CALL(mock_sender, get_own_component_id())
.WillByDefault(Return(own_address.component_id));
ON_CALL(mock_sender, get_system_id()).WillByDefault(Return(target_address.system_id));
ON_CALL(mock_sender, autopilot()).WillByDefault(Return(Sender::Autopilot::Px4));
ON_CALL(mock_sender, compatibility_mode())
.WillByDefault(Return(System::CompatibilityMode::Px4));
}

MockSender mock_sender;
Expand Down
16 changes: 9 additions & 7 deletions src/mavsdk/core/mavlink_parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -193,7 +193,8 @@ void MAVLinkParameters::set_param_int_async(

// PX4 only uses int32_t, so we can be sure and don't need to check the exact type first
// by getting the param, or checking the cache.
const bool exact_int_type_known = (_sender.autopilot() == SystemImpl::Autopilot::Px4);
const bool exact_int_type_known =
(_sender.compatibility_mode() == System::CompatibilityMode::Px4);

const auto set_step = [=]() {
auto new_work = std::make_shared<WorkItem>(_timeout_s_callback());
Expand Down Expand Up @@ -806,9 +807,10 @@ void MAVLinkParameters::do_work()
param_value_buf.data(),
work->param_value.get_mav_param_ext_type());
} else {
float value_set = (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) ?
work->param_value.get_4_float_bytes_cast() :
work->param_value.get_4_float_bytes_bytewise();
float value_set =
(_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) ?
work->param_value.get_4_float_bytes_cast() :
work->param_value.get_4_float_bytes_bytewise();

mavlink_msg_param_set_pack(
_sender.get_own_system_id(),
Expand Down Expand Up @@ -914,7 +916,7 @@ void MAVLinkParameters::do_work()
work->param_index);
} else {
float param_value;
if (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) {
if (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) {
param_value = work->param_value.get_4_float_bytes_cast();
} else {
param_value = work->param_value.get_4_float_bytes_bytewise();
Expand Down Expand Up @@ -975,7 +977,7 @@ void MAVLinkParameters::process_param_value(const mavlink_message_t& message)
}

ParamValue received_value;
if (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) {
if (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) {
received_value.set_from_mavlink_param_value_cast(param_value);
} else {
received_value.set_from_mavlink_param_value_bytewise(param_value);
Expand Down Expand Up @@ -1098,7 +1100,7 @@ void MAVLinkParameters::notify_param_subscriptions(const mavlink_param_value_t&

ParamValue value;

if (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) {
if (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) {
value.set_from_mavlink_param_value_cast(param_value);
} else {
value.set_from_mavlink_param_value_bytewise(param_value);
Expand Down
2 changes: 1 addition & 1 deletion src/mavsdk/core/mocks/sender_mock.h
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ class MockSender : public Sender {
MOCK_METHOD(uint8_t, get_own_system_id, (), (const, override));
MOCK_METHOD(uint8_t, get_own_component_id, (), (const, override));
MOCK_METHOD(uint8_t, get_system_id, (), (const, override));
MOCK_METHOD(Autopilot, autopilot, (), (const, override));
MOCK_METHOD(System::CompatibilityMode, compatibility_mode, (), (const, override));
};

} // namespace testing
Expand Down
4 changes: 2 additions & 2 deletions src/mavsdk/core/server_component_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -382,10 +382,10 @@ uint8_t ServerComponentImpl::OurSender::get_system_id() const
return current_target_system_id;
}

Sender::Autopilot ServerComponentImpl::OurSender::autopilot() const
System::CompatibilityMode ServerComponentImpl::OurSender::compatibility_mode() const
{
// FIXME: hard-coded to PX4 for now to avoid the dependency into mavsdk_impl.
return Sender::Autopilot::Px4;
return System::CompatibilityMode::Px4;
}

} // namespace mavsdk
2 changes: 1 addition & 1 deletion src/mavsdk/core/server_component_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ class ServerComponentImpl {
[[nodiscard]] uint8_t get_own_system_id() const override;
[[nodiscard]] uint8_t get_own_component_id() const override;
[[nodiscard]] uint8_t get_system_id() const override;
[[nodiscard]] Autopilot autopilot() const override;
[[nodiscard]] System::CompatibilityMode compatibility_mode() const override;

uint8_t current_target_system_id{0};

Expand Down
103 changes: 2 additions & 101 deletions src/mavsdk/core/system_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -236,8 +236,8 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message)
_hitl_enabled = (heartbeat.base_mode & MAV_MODE_FLAG_HIL_ENABLED) != 0;
}
if (heartbeat.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
_flight_mode =
to_flight_mode_from_custom_mode(_autopilot, _vehicle_type, heartbeat.custom_mode);
_flight_mode = to_flight_mode_from_custom_mode(
_compatibility_mode, _vehicle_type, heartbeat.custom_mode);
}

set_connected();
Expand Down Expand Up @@ -955,105 +955,6 @@ FlightMode SystemImpl::get_flight_mode() const
return _flight_mode;
}

SystemImpl::FlightMode SystemImpl::to_flight_mode_from_custom_mode(uint32_t custom_mode)
{
if (compatibility_mode() == System::CompatibilityMode::Ardupilot) {
switch (_vehicle_type) {
case MAV_TYPE::MAV_TYPE_SURFACE_BOAT:
case MAV_TYPE::MAV_TYPE_GROUND_ROVER:
return to_flight_mode_from_ardupilot_rover_mode(custom_mode);
default:
return to_flight_mode_from_ardupilot_copter_mode(custom_mode);
}
} else {
return to_flight_mode_from_px4_mode(custom_mode);
}
}

SystemImpl::FlightMode SystemImpl::to_flight_mode_from_ardupilot_rover_mode(uint32_t custom_mode)
{
switch (static_cast<ardupilot::RoverMode>(custom_mode)) {
case ardupilot::RoverMode::Auto:
return FlightMode::Mission;
case ardupilot::RoverMode::Acro:
return FlightMode::Acro;
case ardupilot::RoverMode::Hold:
return FlightMode::Hold;
case ardupilot::RoverMode::RTL:
return FlightMode::ReturnToLaunch;
case ardupilot::RoverMode::Manual:
return FlightMode::Manual;
case ardupilot::RoverMode::Follow:
return FlightMode::FollowMe;
default:
return FlightMode::Unknown;
}
}
SystemImpl::FlightMode SystemImpl::to_flight_mode_from_ardupilot_copter_mode(uint32_t custom_mode)
{
switch (static_cast<ardupilot::CopterMode>(custom_mode)) {
case ardupilot::CopterMode::Auto:
return FlightMode::Mission;
case ardupilot::CopterMode::Acro:
return FlightMode::Acro;
case ardupilot::CopterMode::Alt_Hold:
case ardupilot::CopterMode::POS_HOLD:
case ardupilot::CopterMode::Flow_Hold:
return FlightMode::Hold;
case ardupilot::CopterMode::RTL:
case ardupilot::CopterMode::Auto_RTL:
return FlightMode::ReturnToLaunch;
case ardupilot::CopterMode::Land:
return FlightMode::Land;
default:
return FlightMode::Unknown;
}
}

SystemImpl::FlightMode SystemImpl::to_flight_mode_from_px4_mode(uint32_t custom_mode)
{
px4::px4_custom_mode px4_custom_mode;
px4_custom_mode.data = custom_mode;

switch (px4_custom_mode.main_mode) {
case px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD:
return FlightMode::Offboard;
case px4::PX4_CUSTOM_MAIN_MODE_MANUAL:
return FlightMode::Manual;
case px4::PX4_CUSTOM_MAIN_MODE_POSCTL:
return FlightMode::Posctl;
case px4::PX4_CUSTOM_MAIN_MODE_ALTCTL:
return FlightMode::Altctl;
case px4::PX4_CUSTOM_MAIN_MODE_RATTITUDE:
return FlightMode::Rattitude;
case px4::PX4_CUSTOM_MAIN_MODE_ACRO:
return FlightMode::Acro;
case px4::PX4_CUSTOM_MAIN_MODE_STABILIZED:
return FlightMode::Stabilized;
case px4::PX4_CUSTOM_MAIN_MODE_AUTO:
switch (px4_custom_mode.sub_mode) {
case px4::PX4_CUSTOM_SUB_MODE_AUTO_READY:
return FlightMode::Ready;
case px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
return FlightMode::Takeoff;
case px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
return FlightMode::Hold;
case px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
return FlightMode::Mission;
case px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL:
return FlightMode::ReturnToLaunch;
case px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND:
return FlightMode::Land;
case px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET:
return FlightMode::FollowMe;
default:
return FlightMode::Unknown;
}
default:
return FlightMode::Unknown;
}
}

void SystemImpl::receive_float_param(
MAVLinkParameters::Result result,
MAVLinkParameters::ParamValue value,
Expand Down
6 changes: 3 additions & 3 deletions src/mavsdk/plugins/action/action_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -364,7 +364,7 @@ void ActionImpl::shutdown_async(const Action::ResultCallback& callback) const

void ActionImpl::takeoff_async(const Action::ResultCallback& callback) const
{
if (_parent->autopilot() == SystemImpl::Autopilot::Px4) {
if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) {
takeoff_async_px4(callback);
} else {
takeoff_async_apm(callback);
Expand Down Expand Up @@ -466,9 +466,9 @@ void ActionImpl::goto_location_async(

if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) {
// Change to Hold mode first
if (_parent->get_flight_mode() != SystemImpl::FlightMode::Hold) {
if (_parent->get_flight_mode() != FlightMode::Hold) {
_parent->set_flight_mode_async(
SystemImpl::FlightMode::Hold,
FlightMode::Hold,
[this, callback, send_do_reposition](MavlinkCommandSender::Result result, float) {
Action::Result action_result = action_result_from_command_result(result);
if (action_result != Action::Result::Success) {
Expand Down

0 comments on commit 2ed0b7a

Please sign in to comment.