From f67c092d6aa949f93621a90f0a4cfdd384fd0932 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Fri, 30 Dec 2016 15:00:53 +0100 Subject: [PATCH] LP-462 MWOSD - MSP: Handle Dterm scale --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index cb0c3fb14..c02cc4687 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -676,14 +676,18 @@ static void msp_send_pidnames(struct msp_bridge *m) static void pid_native2msp(const float *native, msp_pid_t *piditem, float scale, unsigned numelem) { for (unsigned i = 0; i < numelem; ++i) { - piditem->values[i] = lroundf(native[i] * scale); + // Handle Dterm scale + float s = (i == 2) ? scale * 100 : scale; + piditem->values[i] = lroundf(native[i] * s); } } static void pid_msp2native(const msp_pid_t *piditem, float *native, float scale, unsigned numelem) { for (unsigned i = 0; i < numelem; ++i) { - native[i] = (float)piditem->values[i] / scale; + // Handle Dterm scale + float s = (i == 2) ? scale * 100 : scale; + native[i] = (float)piditem->values[i] / s; } }