bool set_home_cb(mavros::CommandHome::Request &req,
mavros::CommandHome::Response &res) {
return send_command_long_and_wait(MAV_CMD_DO_SET_HOME, 1,
(req.current_gps)? 1.0 : 0.0,
0, 0, 0, req.latitude, req.longitude, req.altitude,
res.success, res.result);
}
bool arming_cb(mavros::CommandBool::Request &req,
mavros::CommandBool::Response &res) {
return send_command_long_and_wait(MAV_CMD_COMPONENT_ARM_DISARM, 1,
(req.value)? 1.0 : 0.0,
0, 0, 0, 0, 0, 0,
res.success, res.result);
}
bool guided_cb(mavros::CommandBool::Request &req,
mavros::CommandBool::Response &res) {
return send_command_long_and_wait(false,
MAV_CMD_NAV_GUIDED_ENABLE, 1,
(req.value) ? 1.0 : 0.0,
0, 0, 0, 0, 0, 0,
res.success, res.result);
}
bool land_cb(mavros::CommandTOL::Request &req,
mavros::CommandTOL::Response &res) {
return send_command_long_and_wait(MAV_CMD_NAV_LAND, 1,
0, 0, 0,
req.yaw,
req.latitude, req.longitude, req.altitude,
res.success, res.result);
}
bool takeoff_cb(mavros::CommandTOL::Request &req,
mavros::CommandTOL::Response &res) {
return send_command_long_and_wait(MAV_CMD_NAV_TAKEOFF, 1,
req.min_pitch,
0, 0,
req.yaw,
req.latitude, req.longitude, req.altitude,
res.success, res.result);
}
bool command_long_cb(mavros::CommandLong::Request &req,
mavros::CommandLong::Response &res) {
return send_command_long_and_wait(req.command, req.confirmation,
req.param1, req.param2,
req.param3, req.param4,
req.param5, req.param6,
req.param7,
res.success, res.result);
}
bool set_mode_cb(mavros::CommandMode::Request &req,
mavros::CommandMode::Response &res) {
if (req.mode > 256) {
ROS_ERROR_NAMED("cmd", "Unknown mode %u", req.mode);
return false;
}
/* TODO: Add FCU-specific mode set
* like APM LAND,TAKEOFF and other
*/
return send_command_long_and_wait(MAV_CMD_DO_SET_MODE, 1,
req.mode,
0, 0, 0, 0, 0, 0,
res.success, res.result);
}
