Revision 493cca7c
| ID | 493cca7c321081c9bdcb1cf72714506ef1310954 |
basic (not-yet-working) flight control via keyboard
| mikrokopter/src/nav_lib.cpp | ||
|---|---|---|
| 1 | 1 |
#include "nav_lib.h" |
| 2 | 2 |
|
| 3 |
void set_roll_pitch(ros::Publisher& pub, int roll, int pitch) {
|
|
| 4 |
mikrokopter::Control req; |
|
| 3 |
|
|
| 4 |
void init_control_msg(mikrokopter::Control& req) |
|
| 5 |
{
|
|
| 5 | 6 |
req.digital[0] = 0; |
| 6 | 7 |
req.digital[1] = 0; |
| 7 | 8 |
req.remoteKey = 0; |
| 8 |
req.pitch = roll;
|
|
| 9 |
req.roll = pitch;
|
|
| 9 |
req.pitch = 0;
|
|
| 10 |
req.roll = 0;
|
|
| 10 | 11 |
req.yaw = 0; |
| 11 | 12 |
req.thrust = 40; |
| 12 | 13 |
req.height = 0; |
| 13 | 14 |
req.free = 0; |
| 14 | 15 |
req.frame = 7; |
| 15 | 16 |
req.config = 1; |
| 16 |
pub.publish(req); |
|
| 17 |
ros::spinOnce(); |
|
| 18 | 17 |
} |
| 19 | 18 |
|
| 20 |
|
|
| 21 |
void forward(ros::Publisher& pub) |
|
| 19 |
void forward(mikrokopter::Control& req) |
|
| 22 | 20 |
{
|
| 23 |
set_roll_pitch(pub, -15, -15); |
|
| 21 |
req.roll = -15; |
|
| 22 |
req.pitch = -15; |
|
| 24 | 23 |
} |
| 25 | 24 |
|
| 26 |
void backward(ros::Publisher& pub)
|
|
| 25 |
void backward(mikrokopter::Control& req)
|
|
| 27 | 26 |
{
|
| 28 |
set_roll_pitch(pub, 15, 15); |
|
| 27 |
req.roll = 15; |
|
| 28 |
req.pitch = 15; |
|
| 29 | 29 |
} |
| 30 | 30 |
|
| 31 |
void left(ros::Publisher& pub)
|
|
| 31 |
void left(mikrokopter::Control& req)
|
|
| 32 | 32 |
{
|
| 33 |
set_roll_pitch(pub, -15, 15); |
|
| 33 |
req.roll = -15; |
|
| 34 |
req.pitch = 15; |
|
| 34 | 35 |
} |
| 35 | 36 |
|
| 36 |
void right(ros::Publisher& pub)
|
|
| 37 |
void right(mikrokopter::Control& req)
|
|
| 37 | 38 |
{
|
| 38 |
set_roll_pitch(pub, 15, -15); |
|
| 39 |
req.roll = 15; |
|
| 40 |
req.pitch = -15; |
|
| 39 | 41 |
} |
| 40 | 42 |
|
| 41 |
void stop(ros::Publisher& pub)
|
|
| 43 |
void set_thrust(mikrokopter::Control& req, int thrust)
|
|
| 42 | 44 |
{
|
| 43 |
set_roll_pitch(pub, 0, 0);
|
|
| 45 |
req.thrust = thrust;
|
|
| 44 | 46 |
} |
| 47 |
|
|
Also available in: Unified diff