Quadrotor

#include "nav_lib.h"

void set_roll_pitch(ros::Publisher& pub, int roll, int pitch) {

    mikrokopter::Control req;

    req.digital[0] = 0;

    req.digital[1] = 0;

    req.remoteKey = 0;

    req.pitch = roll;

    req.roll = pitch;

    req.yaw = 0;

    req.thrust = 40;

    req.height = 0;

    req.free = 0;

    req.frame = 7;

    req.config = 1;

    pub.publish(req);

    ros::spinOnce();

}

void forward(ros::Publisher& pub)

{

    set_roll_pitch(pub, -15, -15);

}

void backward(ros::Publisher& pub)

{

    set_roll_pitch(pub, 15, 15);

}

void left(ros::Publisher& pub)

{

    set_roll_pitch(pub, -15, 15);

}

void right(ros::Publisher& pub)

{

    set_roll_pitch(pub, 15, -15);

}

void stop(ros::Publisher& pub)

{

    set_roll_pitch(pub, 0, 0);

}