Quadrotor

#include "nav_lib.h"

MikrokopterControl::MikrokopterControl()

{

    control.digital[0] = 0;

    control.digital[1] = 0;

    control.remoteKey = 0;

    control.pitch = 0;

    control.roll = 0;

    control.yaw = 0;

    control.thrust = 40;

    control.height = 0;

    control.free = 0;

    control.frame = 7;

    control.config = 1;

}

void MikrokopterControl::velocity_control(float forward_speed, float lateral_speed)

{

    if(forward_speed > 1) forward_speed = 1;

    if(forward_speed < -1) forward_speed = -1;

    if(lateral_speed > 1) lateral_speed = 1;

    if(lateral_speed < -1) lateral_speed = -1; 

    int k = 50; // TODO set scaling constant and check signs

    control.roll = k*(forward_speed - lateral_speed);

    control.pitch = k*(forward_speed + lateral_speed);

}

void MikrokopterControl::forward()

{

    control.roll = 15;

    control.pitch = 15;

}

void MikrokopterControl::backward()

{

    control.roll = -15;

    control.pitch = -15;

}

void MikrokopterControl::left()

{

    control.roll = 15;

    control.pitch = -15;

}

void MikrokopterControl::right()

{

    control.roll = -15;

    control.pitch = 15;

}

void MikrokopterControl::level()

{

    control.roll = 0;

    control.pitch = 0;

}

void MikrokopterControl::set_thrust(float thrust)

{

    if (thrust < 0) thrust = 0;

    if (thrust > 1) thrust = 1;

    int k = 255;

    control.thrust = k*thrust;

}

void MikrokopterControl::set_yaw(float yaw)

{

    if(yaw>1) yaw = 1;

    if(yaw<-1) yaw = -1;

    int k = 50;;

    control.yaw = k*yaw;

}

void MikrokopterControl::publish_on(ros::Publisher& pub)

{

    pub.publish(control);

}

void MikrokopterControl::main_loop()

{

    ros::Publisher pub = nh.advertise< ::mikrokopter::Control>(

        "/mk_wrapper/control", 200);

    int publish_rate;

    ros::param::param<int>("~publish_rate", publish_rate, 20);

    ros::Rate loop_rate(publish_rate);

    while (ros::ok())

    {

        ros::spinOnce();

        pub.publish(control);

        loop_rate.sleep();

    }

}