diff --git a/core/device_impl.cpp b/core/device_impl.cpp index a929458953..1bd073a6b7 100644 --- a/core/device_impl.cpp +++ b/core/device_impl.cpp @@ -3,6 +3,7 @@ #include "dronecore_impl.h" #include "mavlink_include.h" #include +#include "px4_custom_mode.h" // Set to 1 to log incoming/outgoing mavlink messages. #define MESSAGE_DEBUGGING 0 @@ -440,6 +441,107 @@ void DeviceImpl::get_param_float_async(const std::string &name, callback)); } +MavlinkCommands::Result DeviceImpl::set_flight_mode(FlightMode device_mode) +{ + uint8_t flag_safety_armed = is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; + + uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; + + // Note: the safety flag is not needed in future versions of the PX4 Firmware + // but want to be rather safe than sorry. + uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; + uint8_t custom_sub_mode = 0; + + switch (device_mode) { + case FlightMode::HOLD: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; + case FlightMode::RETURN_TO_LAUNCH: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; + case FlightMode::TAKEOFF: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + break; + case FlightMode::LAND: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND; + break; + case FlightMode::MISSION: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; + case FlightMode::FOLLOW_ME: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET; + break; + case FlightMode::OFFBOARD: + custom_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD; + break; + default : + LogErr() << "Unknown Flight mode."; + return MavlinkCommands::Result::ERROR; + + } + + MavlinkCommands::Result ret = send_command_with_ack( + MAV_CMD_DO_SET_MODE, + MavlinkCommands::Params {float(mode), + float(custom_mode), + float(custom_sub_mode), + NAN, NAN, NAN, NAN}, + MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); + + return ret; +} + +void DeviceImpl::set_flight_mode_async(FlightMode device_mode, command_result_callback_t callback) +{ + uint8_t flag_safety_armed = is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; + + uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; + + // Note: the safety flag is not needed in future versions of the PX4 Firmware + // but want to be rather safe than sorry. + uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; + uint8_t custom_sub_mode = 0; + + switch (device_mode) { + case FlightMode::HOLD: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; + case FlightMode::RETURN_TO_LAUNCH: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; + case FlightMode::TAKEOFF: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + break; + case FlightMode::LAND: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND; + break; + case FlightMode::MISSION: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; + case FlightMode::FOLLOW_ME: + custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET; + break; + case FlightMode::OFFBOARD: + custom_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD; + break; + default : + LogErr() << "Unknown flight mode."; + if (callback) { + callback(MavlinkCommands::Result::ERROR, NAN); + } + return ; + } + + + send_command_with_ack_async(MAV_CMD_DO_SET_MODE, + MavlinkCommands::Params {float(mode), + float(custom_mode), + float(custom_sub_mode), + NAN, NAN, NAN, NAN}, + callback, + MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); +} + void DeviceImpl::get_param_int_async(const std::string &name, get_param_int_callback_t callback) { diff --git a/core/device_impl.h b/core/device_impl.h index 6078683624..13929643d7 100644 --- a/core/device_impl.h +++ b/core/device_impl.h @@ -18,10 +18,19 @@ namespace dronecore { class DroneCoreImpl; - class DeviceImpl { public: + enum class FlightMode { + HOLD = 0, + RETURN_TO_LAUNCH, + TAKEOFF, + LAND, + MISSION, + FOLLOW_ME, + OFFBOARD, + }; + explicit DeviceImpl(DroneCoreImpl *parent, uint8_t target_system_id); ~DeviceImpl(); @@ -79,6 +88,8 @@ class DeviceImpl void set_param_int_async(const std::string &name, int32_t value, success_t callback); void set_param_ext_float_async(const std::string &name, float value, success_t callback); void set_param_ext_int_async(const std::string &name, int32_t value, success_t callback); + MavlinkCommands::Result set_flight_mode(FlightMode mode); + void set_flight_mode_async(FlightMode mode, command_result_callback_t callback); typedef std::function get_param_float_callback_t; typedef std::function get_param_int_callback_t; diff --git a/core/mavlink_commands.h b/core/mavlink_commands.h index 028eda1809..8b93c1e7be 100644 --- a/core/mavlink_commands.h +++ b/core/mavlink_commands.h @@ -24,7 +24,8 @@ class MavlinkCommands BUSY, COMMAND_DENIED, TIMEOUT, - IN_PROGRESS + IN_PROGRESS, + ERROR }; typedef std::function command_result_callback_t; diff --git a/plugins/action/action_impl.cpp b/plugins/action/action_impl.cpp index f803c99069..fd7cfce55b 100644 --- a/plugins/action/action_impl.cpp +++ b/plugins/action/action_impl.cpp @@ -45,20 +45,8 @@ Action::Result ActionImpl::arm() const } // Go to LOITER mode first. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - ret = action_result_from_command_result( - _parent->send_command_with_ack( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT)); + _parent->set_flight_mode(DeviceImpl::FlightMode::HOLD)); if (ret != Action::Result::SUCCESS) { return ret; @@ -102,20 +90,8 @@ Action::Result ActionImpl::takeoff() const } // Go to LOITER mode first. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - ret = action_result_from_command_result( - _parent->send_command_with_ack( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT)); + _parent->set_flight_mode(DeviceImpl::FlightMode::HOLD)); return action_result_from_command_result( _parent->send_command_with_ack( @@ -136,23 +112,8 @@ Action::Result ActionImpl::land() const Action::Result ActionImpl::return_to_launch() const { - // Note: the safety flag is not needed in future versions of the PX4 Firmware - // but want to be rather safe than sorry. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED | - flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL; - return action_result_from_command_result( - _parent->send_command_with_ack( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT)); + _parent->set_flight_mode(DeviceImpl::FlightMode::RETURN_TO_LAUNCH)); } Action::Result ActionImpl::transition_to_fixedwing() const @@ -345,25 +306,9 @@ void ActionImpl::land_async(const Action::result_callback_t &callback) void ActionImpl::return_to_launch_async(const Action::result_callback_t &callback) { - // Note: the safety flag is not needed in future versions of the PX4 Firmware - // but want to be rather safe than sorry. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED | - flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL; - - _parent->send_command_with_ack_async( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, - std::bind(&ActionImpl::command_result_callback, - _1, - callback), - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); + _parent->set_flight_mode_async( + DeviceImpl::FlightMode::RETURN_TO_LAUNCH, + std::bind(&ActionImpl::command_result_callback, _1, callback)); } Action::Result ActionImpl::arming_allowed() const @@ -426,40 +371,16 @@ void ActionImpl::process_extended_sys_state(const mavlink_message_t &message) void ActionImpl::loiter_before_takeoff_async(const Action::result_callback_t &callback) { - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - - _parent->send_command_with_ack_async( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, - std::bind(&ActionImpl::takeoff_async_continued, this, _1, - callback), - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); + _parent->set_flight_mode_async( + DeviceImpl::FlightMode::HOLD, + std::bind(&ActionImpl::takeoff_async_continued, this, _1, callback)); } void ActionImpl::loiter_before_arm_async(const Action::result_callback_t &callback) { - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - - _parent->send_command_with_ack_async( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, - std::bind(&ActionImpl::arm_async_continued, this, _1, - callback), - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); + _parent->set_flight_mode_async( + DeviceImpl::FlightMode::HOLD, + std::bind(&ActionImpl::arm_async_continued, this, _1, callback)); } diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp index 166870403a..5cff7dcc3a 100644 --- a/plugins/follow_me/follow_me_impl.cpp +++ b/plugins/follow_me/follow_me_impl.cpp @@ -144,23 +144,9 @@ bool FollowMeImpl::is_active() const FollowMe::Result FollowMeImpl::start() { - // Note: the safety flag is not needed in future versions of the PX4 Firmware - // but want to be rather safe than sorry. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET; - - FollowMe::Result result = to_follow_me_result( - _parent->send_command_with_ack( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT)); + _parent->set_flight_mode( + DeviceImpl::FlightMode::FOLLOW_ME)); if (result == FollowMe::Result::SUCCESS) { // If location was set before, lets send it to vehicle @@ -183,22 +169,9 @@ FollowMe::Result FollowMeImpl::stop() stop_sending_target_location(); } } - // Note: the safety flag is not needed in future versions of the PX4 Firmware - // but want to be rather safe than sorry. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - return to_follow_me_result( - _parent->send_command_with_ack( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT)); + _parent->set_flight_mode( + DeviceImpl::FlightMode::HOLD)); } // Applies default FollowMe configuration to the device diff --git a/plugins/mission/mission_impl.cpp b/plugins/mission/mission_impl.cpp index 03bd988d5a..033648403f 100644 --- a/plugins/mission/mission_impl.cpp +++ b/plugins/mission/mission_impl.cpp @@ -669,25 +669,12 @@ void MissionImpl::start_mission_async(const Mission::result_callback_t &callback return; } - // Note: the safety flag is not needed in future versions of the PX4 Firmware - // but want to be rather safe than sorry. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - _activity = Activity::SEND_COMMAND; - _parent->send_command_with_ack_async( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, + _parent->set_flight_mode_async( + DeviceImpl::FlightMode::MISSION, std::bind(&MissionImpl::receive_command_result, this, - std::placeholders::_1, callback), - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); + std::placeholders::_1, callback)); _result_callback = callback; } @@ -701,25 +688,12 @@ void MissionImpl::pause_mission_async(const Mission::result_callback_t &callback return; } - // Note: the safety flag is not needed in future versions of the PX4 Firmware - // but want to be rather safe than sorry. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - _activity = Activity::SEND_COMMAND; - _parent->send_command_with_ack_async( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, + _parent->set_flight_mode_async( + DeviceImpl::FlightMode::HOLD, std::bind(&MissionImpl::receive_command_result, this, - std::placeholders::_1, callback), - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); + std::placeholders::_1, callback)); _result_callback = callback; } diff --git a/plugins/offboard/offboard_impl.cpp b/plugins/offboard/offboard_impl.cpp index 5a0aa77212..48b337f158 100644 --- a/plugins/offboard/offboard_impl.cpp +++ b/plugins/offboard/offboard_impl.cpp @@ -36,22 +36,8 @@ Offboard::Result OffboardImpl::start() } } - // Note: the safety flag is not needed in future versions of the PX4 Firmware - // but want to be rather safe than sorry. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD; - uint8_t custom_sub_mode = 0; - return offboard_result_from_command_result( - _parent->send_command_with_ack( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT)); + _parent->set_flight_mode(DeviceImpl::FlightMode::OFFBOARD)); } Offboard::Result OffboardImpl::stop() @@ -63,22 +49,8 @@ Offboard::Result OffboardImpl::stop() } } - // Note: the safety flag is not needed in future versions of the PX4 Firmware - // but want to be rather safe than sorry. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - return offboard_result_from_command_result( - _parent->send_command_with_ack( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT)); + _parent->set_flight_mode(DeviceImpl::FlightMode::HOLD)); } void OffboardImpl::start_async(Offboard::result_callback_t callback) @@ -94,23 +66,10 @@ void OffboardImpl::start_async(Offboard::result_callback_t callback) } } - // Note: the safety flag is not needed in future versions of the PX4 Firmware - // but want to be rather safe than sorry. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD; - uint8_t custom_sub_mode = 0; - - _parent->send_command_with_ack_async( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, + _parent->set_flight_mode_async( + DeviceImpl::FlightMode::OFFBOARD, std::bind(&OffboardImpl::receive_command_result, this, - std::placeholders::_1, callback), - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); + std::placeholders::_1, callback)); } void OffboardImpl::stop_async(Offboard::result_callback_t callback) @@ -122,23 +81,10 @@ void OffboardImpl::stop_async(Offboard::result_callback_t callback) } } - // Note: the safety flag is not needed in future versions of the PX4 Firmware - // but want to be rather safe than sorry. - uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; - - uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed; - uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO; - uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - - _parent->send_command_with_ack_async( - MAV_CMD_DO_SET_MODE, - MavlinkCommands::Params {float(mode), - float(custom_mode), - float(custom_sub_mode), - NAN, NAN, NAN, NAN}, + _parent->set_flight_mode_async( + DeviceImpl::FlightMode::HOLD, std::bind(&OffboardImpl::receive_command_result, this, - std::placeholders::_1, callback), - MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); + std::placeholders::_1, callback)); } bool OffboardImpl::is_active() const