From 1b1bc9f3b15dc881d1755298de4eb0e5f83eadcb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 27 Jul 2015 10:05:33 +1000 Subject: [PATCH] RC_Channel: prevent floating exception on bad MIN/MAX/TRIM values --- libraries/RC_Channel/RC_Channel.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 8f351758e5..2ec7b2e285 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -428,10 +428,16 @@ RC_Channel::norm_output() { int16_t mid = (radio_max + radio_min) / 2; float ret; - if(radio_out < mid) + if (mid <= radio_min) { + return 0; + } + if (radio_out < mid) { ret = (float)(radio_out - mid) / (float)(mid - radio_min); - else + } else if (radio_out > mid) { ret = (float)(radio_out - mid) / (float)(radio_max - mid); + } else { + ret = 0; + } if (_reverse == -1) { ret = -ret; }