Revision 84a809d3
ID | 84a809d3edf1be1ee41414e9dd6c9850d8758e62 |
Modified nav_lib to cap input for set_velocity and set_yaw between -1 and 1. Also modified CoordToPID to pass timestamp to PID.
mikrokopter/src/nav_lib.cpp | ||
---|---|---|
17 | 17 |
|
18 | 18 |
void MikrokopterControl::velocity_control(float forward_speed, float lateral_speed) |
19 | 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; |
|
20 | 24 |
int k = 50; // TODO set scaling constant and check signs |
21 | 25 |
control.roll = k*(forward_speed - lateral_speed); |
22 | 26 |
control.pitch = k*(forward_speed + lateral_speed); |
... | ... | |
60 | 64 |
|
61 | 65 |
void MikrokopterControl::set_yaw(float yaw) |
62 | 66 |
{ |
63 |
int k = 50; |
|
67 |
if(yaw>1) yaw = 1; |
|
68 |
if(yaw<-1) yaw = -1; |
|
69 |
int k = 50;; |
|
64 | 70 |
control.yaw = k*yaw; |
65 | 71 |
} |
66 | 72 |
|
Also available in: Unified diff