Statistics
| Branch: | Revision:

root / quad2 / mikrokopter / src / nav_lib.cpp @ 98e35b23

History | View | Annotate | Download (1.95 KB)

1
#include "nav_lib.h"
2

    
3
MikrokopterControl::MikrokopterControl()
4
{
5
    control.digital[0] = 0;
6
    control.digital[1] = 0;
7
    control.remoteKey = 0;
8
    control.pitch = 0;
9
    control.roll = 0;
10
    control.yaw = 0;
11
    control.thrust = 40;
12
    control.height = 0;
13
    control.free = 0;
14
    control.frame = 7;
15
    control.config = 1;
16
}
17

    
18
void MikrokopterControl::velocity_control(float forward_speed, float lateral_speed)
19
{
20
    if(forward_speed > 1) forward_speed = 1;
21
    if(forward_speed < -1) forward_speed = -1;
22
    if(lateral_speed > 1) lateral_speed = 1;
23
    if(lateral_speed < -1) lateral_speed = -1; 
24
    int k = 50; // TODO set scaling constant and check signs
25
    control.roll = k*(forward_speed - lateral_speed);
26
    control.pitch = k*(forward_speed + lateral_speed);
27
}
28

    
29
void MikrokopterControl::forward()
30
{
31
    control.roll = 15;
32
    control.pitch = 15;
33
}
34

    
35
void MikrokopterControl::backward()
36
{
37
    control.roll = -15;
38
    control.pitch = -15;
39
}
40

    
41
void MikrokopterControl::left()
42
{
43
    control.roll = 15;
44
    control.pitch = -15;
45
}
46

    
47
void MikrokopterControl::right()
48
{
49
    control.roll = -15;
50
    control.pitch = 15;
51
}
52

    
53
void MikrokopterControl::level()
54
{
55
    control.roll = 0;
56
    control.pitch = 0;
57
}
58

    
59
void MikrokopterControl::set_thrust(float thrust)
60
{
61
    if (thrust < 0) thrust = 0;
62
    if (thrust > 1) thrust = 1;
63
    int k = 255;
64
    control.thrust = k*thrust;
65
}
66

    
67
void MikrokopterControl::set_yaw(float yaw)
68
{
69
    if(yaw>1) yaw = 1;
70
    if(yaw<-1) yaw = -1;
71
    int k = 50;;
72
    control.yaw = k*yaw;
73
}
74

    
75
void MikrokopterControl::publish_on(ros::Publisher& pub)
76
{
77
    pub.publish(control);
78
}
79

    
80
void MikrokopterControl::main_loop()
81
{
82
    ros::Publisher pub = nh.advertise< ::mikrokopter::Control>(
83
        "/mk_wrapper/control", 200);
84
    
85
    int publish_rate;
86
    ros::param::param<int>("~publish_rate", publish_rate, 20);
87
    ros::Rate loop_rate(publish_rate);
88
    
89
    while (ros::ok())
90
    {
91
        ros::spinOnce();
92
        pub.publish(control);
93
        loop_rate.sleep();
94
    }
95
    
96
}