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