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

Jump to bottom

Set Flight Mode using new interface set_flight_mode in DeviceImpl #206

Merged
merged 4 commits into from
Jan 15, 2018
Merged

Set Flight Mode using new interface set_flight_mode in DeviceImpl #206

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
f84faaf
Set Flight Mode using new interface set_flight_mode in DeviceImpl
Rjasuja Dec 22, 2017
cd5b6f3
Simplify set_flight_mode API
Rjasuja Jan 2, 2018
8cb72f8
Update DeviceImpl
Rjasuja Jan 5, 2018
981f6f8
Change Error handling: UNSUPPORTED_MODE => ERROR
Rjasuja Jan 8, 2018
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
102 changes: 102 additions & 0 deletions core/device_impl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include "dronecore_impl.h"
#include "mavlink_include.h"
#include <functional>
#include "px4_custom_mode.h"

// Set to 1 to log incoming/outgoing mavlink messages.
#define MESSAGE_DEBUGGING 0
Expand Down Expand Up @@ -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 :
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In this case it would be helpful to add a LogErr() << "Unknown Flight mode chosen.";

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)
{
Expand Down
13 changes: 12 additions & 1 deletion core/device_impl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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 <void(bool success, float value)> get_param_float_callback_t;
typedef std::function <void(bool success, int32_t value)> get_param_int_callback_t;
Expand Down
3 changes: 2 additions & 1 deletion core/mavlink_commands.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,8 @@ class MavlinkCommands
BUSY,
COMMAND_DENIED,
TIMEOUT,
IN_PROGRESS
IN_PROGRESS,
ERROR
};

typedef std::function<void(Result, float)> command_result_callback_t;
Expand Down
103 changes: 12 additions & 91 deletions plugins/action/action_impl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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(
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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));
}


Expand Down
35 changes: 4 additions & 31 deletions plugins/follow_me/follow_me_impl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
Loading