mirror of
https://github.com/okalachev/flix.git
synced 2026-01-09 12:36:49 +00:00
Move yaw dead zone handling from mavlink to control subsystem
So yaw dead zone works the same for rc and mavlink.
This commit is contained in:
@@ -75,6 +75,8 @@ void interpretControls() {
|
||||
if (controlThrottle < 0.05 && controlYaw > 0.95) armed = true; // arm gesture
|
||||
if (controlThrottle < 0.05 && controlYaw < -0.95) armed = false; // disarm gesture
|
||||
|
||||
if (abs(controlYaw) < 0.1) controlYaw = 0; // yaw dead zone
|
||||
|
||||
thrustTarget = controlThrottle;
|
||||
|
||||
if (mode == STAB) {
|
||||
|
||||
@@ -11,7 +11,6 @@
|
||||
#define SYSTEM_ID 1
|
||||
#define MAVLINK_RATE_SLOW 1
|
||||
#define MAVLINK_RATE_FAST 10
|
||||
#define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f
|
||||
|
||||
bool mavlinkConnected = false;
|
||||
String mavlinkPrintBuffer;
|
||||
@@ -105,8 +104,6 @@ void handleMavlink(const void *_msg) {
|
||||
controlYaw = m.r / 1000.0f;
|
||||
controlMode = NAN;
|
||||
controlTime = t;
|
||||
|
||||
if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
|
||||
}
|
||||
|
||||
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
|
||||
|
||||
Reference in New Issue
Block a user