Skip to content

Commit

Permalink
Change Error handling: UNSUPPORTED_MODE => ERROR
Browse files Browse the repository at this point in the history
  • Loading branch information
@Rjasuja @julianoes
Rjasuja authored and julianoes committed Jan 15, 2018
1 parent a7cde06 commit b6897ee
Show file tree
Hide file tree
Showing 4 changed files with 37 additions and 39 deletions.
53 changes: 28 additions & 25 deletions core/device_impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -453,29 +453,30 @@ MavlinkCommands::Result DeviceImpl::set_flight_mode(FlightMode device_mode)
uint8_t custom_sub_mode = 0;

switch (device_mode) {
case FlightMode::HOLD :
case FlightMode::HOLD:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
break;
case FlightMode::RETURN_TO_LAUNCH :
case FlightMode::RETURN_TO_LAUNCH:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
case FlightMode::TAKEOFF :
case FlightMode::TAKEOFF:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
break;
case FlightMode::LAND :
case FlightMode::LAND:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND;
break;
case FlightMode::MISSION :
case FlightMode::MISSION:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
case FlightMode::FOLLOW_ME :
case FlightMode::FOLLOW_ME:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
break;
case FlightMode::OFFBOARD :
case FlightMode::OFFBOARD:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD;
break;
default :
return MavlinkCommands::Result::UNSUPPORTED_MODE;
LogErr() << "Unknown Flight mode.";
return MavlinkCommands::Result::ERROR;

}

Expand All @@ -490,8 +491,7 @@ MavlinkCommands::Result DeviceImpl::set_flight_mode(FlightMode device_mode)
return ret;
}

void DeviceImpl::set_flight_mode_async(FlightMode device_mode,
command_result_callback_t callback)
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;

Expand All @@ -503,40 +503,43 @@ void DeviceImpl::set_flight_mode_async(FlightMode device_mode,
uint8_t custom_sub_mode = 0;

switch (device_mode) {
case FlightMode::HOLD :
case FlightMode::HOLD:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
break;
case FlightMode::RETURN_TO_LAUNCH :
case FlightMode::RETURN_TO_LAUNCH:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
case FlightMode::TAKEOFF :
case FlightMode::TAKEOFF:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
break;
case FlightMode::LAND :
case FlightMode::LAND:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND;
break;
case FlightMode::MISSION :
case FlightMode::MISSION:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
case FlightMode::FOLLOW_ME :
case FlightMode::FOLLOW_ME:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
break;
case FlightMode::OFFBOARD :
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);
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,
Expand Down
3 changes: 1 addition & 2 deletions core/device_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,8 +89,7 @@ class DeviceImpl
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);
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
2 changes: 1 addition & 1 deletion core/mavlink_commands.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ class MavlinkCommands
COMMAND_DENIED,
TIMEOUT,
IN_PROGRESS,
UNSUPPORTED_MODE
ERROR
};

typedef std::function<void(Result, float)> command_result_callback_t;
Expand Down
18 changes: 7 additions & 11 deletions plugins/action/action_impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,8 +90,8 @@ Action::Result ActionImpl::takeoff() const
}

// Go to LOITER mode first.
ret = action_result_from_command_result(_parent->set_flight_mode(
DeviceImpl::FlightMode::HOLD));
ret = action_result_from_command_result(
_parent->set_flight_mode(DeviceImpl::FlightMode::HOLD));

return action_result_from_command_result(
_parent->send_command_with_ack(
Expand All @@ -112,8 +112,8 @@ Action::Result ActionImpl::land() const

Action::Result ActionImpl::return_to_launch() const
{
return action_result_from_command_result(_parent->set_flight_mode(
DeviceImpl::FlightMode::RETURN_TO_LAUNCH));
return action_result_from_command_result(
_parent->set_flight_mode(DeviceImpl::FlightMode::RETURN_TO_LAUNCH));
}

Action::Result ActionImpl::transition_to_fixedwing() const
Expand Down Expand Up @@ -308,9 +308,7 @@ void ActionImpl::return_to_launch_async(const Action::result_callback_t &callbac
{
_parent->set_flight_mode_async(
DeviceImpl::FlightMode::RETURN_TO_LAUNCH,
std::bind(&ActionImpl::command_result_callback,
_1,
callback));
std::bind(&ActionImpl::command_result_callback, _1, callback));
}

Action::Result ActionImpl::arming_allowed() const
Expand Down Expand Up @@ -375,16 +373,14 @@ void ActionImpl::loiter_before_takeoff_async(const Action::result_callback_t &ca
{
_parent->set_flight_mode_async(
DeviceImpl::FlightMode::HOLD,
std::bind(&ActionImpl::takeoff_async_continued, this, _1,
callback));
std::bind(&ActionImpl::takeoff_async_continued, this, _1, callback));
}

void ActionImpl::loiter_before_arm_async(const Action::result_callback_t &callback)
{
_parent->set_flight_mode_async(
DeviceImpl::FlightMode::HOLD,
std::bind(&ActionImpl::arm_async_continued, this, _1,
callback));
std::bind(&ActionImpl::arm_async_continued, this, _1, callback));
}


Expand Down

0 comments on commit b6897ee

Please sign in to comment.