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); }