Here is essentially the code I have been using for this purpose.
You don't necessarily need to use max priority for sp.sched_priority.
There may be better ways of doing this, I guess... if anyone sees if
I'm missing something. Seems to work, but has the disadvantage of
having to run under "sudo" to gain the priority.
I'm not 100% sure if the mlockall call is necessary.
void set_process_highpriority()
{
/* Lock memory for better realtime response */
mlockall(MCL_FUTURE | MCL_CURRENT);
/* Set realtime scheduler policy, with a high priority. */
sched_param sp;
sched_getparam(getpid(), &sp);
sp.sched_priority = sched_get_priority_min(SCHED_FIFO);
sched_setscheduler(getpid(), SCHED_FIFO, &sp);
if (sched_getscheduler(getpid()) != SCHED_FIFO)
printf
("Warning: Could not set realtime scheduler policy.
Servoloop may be slow.\n"
" Try running as superuser.\n");
}