//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
// $Id$
# include <sys/types.h>
# include <sys/stat.h>
# include <fcntl.h>
+# include <unistd.h>
+# include <istream>
#endif
#include <errno.h>
/////////////////////////////////////////////////////////////////////
// scale a number between min and max (with center defined) to a scale
-// from -1.0 to 1.0
-static double scale( int center, int min, int max, int value ) {
+// from -1.0 to 1.0. The deadband value is symmetric, so specifying
+// '1' will give you a deadband of +/-1
+static double scale( int center, int deadband, int min, int max, int value ) {
// cout << center << " " << min << " " << max << " " << value << " ";
double result;
double range;
- if ( value <= center ) {
- range = center - min;
- result = (value - center) / range;
+ if ( value <= (center - deadband) ) {
+ range = (center - deadband) - min;
+ result = (value - (center - deadband)) / range;
+ } else if ( value >= (center + deadband) ) {
+ range = max - (center + deadband);
+ result = (value - (center + deadband)) / range;
} else {
- range = max - center;
- result = (value - center) / range;
+ result = 0.0;
}
if ( result < -1.0 ) result = -1.0;
}
+static double clamp( double min, double max, double value ) {
+ double result = value;
+
+ if ( result < min ) result = min;
+ if ( result > max ) result = max;
+
+ // cout << result << endl;
+
+ return result;
+}
+
+
static int tony_magic( int raw, int obs[3] ) {
int result = 0;
string name = "";
string type = "";
string subtype = "";
- vector <SGPropertyNode *> output_nodes; output_nodes.clear();
+ vector <SGPropertyNode *> output_nodes;
int center = -1;
int min = 0;
int max = 1023;
+ int deadband = 0;
+ float offset = 0.0;
float factor = 1.0;
if ( cname == "channel" ) {
SGPropertyNode *prop;
if ( prop != NULL ) {
max = prop->getIntValue();
}
+ prop = child->getChild( "deadband" );
+ if ( prop != NULL ) {
+ deadband = prop->getIntValue();
+ }
+ prop = child->getChild( "offset" );
+ if ( prop != NULL ) {
+ offset = prop->getFloatValue();
+ }
prop = child->getChild( "factor" );
if ( prop != NULL ) {
factor = prop->getFloatValue();
// "Cook" the raw value
float scaled_value = 0.0f;
if ( center >= 0 ) {
- scaled_value = scale( center, min, max, raw_value );
+ scaled_value = scale( center, deadband,
+ min, max, raw_value );
} else {
scaled_value = scale( min, max, raw_value );
}
scaled_value *= factor;
+ scaled_value += offset;
+
+ // final sanity clamp
+ if ( center >= 0 ) {
+ scaled_value = clamp( -1.0, 1.0, scaled_value );
+ } else {
+ scaled_value = clamp( 0.0, 1.0, scaled_value );
+ }
// update the property tree values
for ( j = 0; j < (int)output_nodes.size(); ++j ) {
// "Cook" the raw value
float scaled_value = 0.0f;
if ( center >= 0 ) {
- scaled_value = scale( center, min, max, raw_value );
+ scaled_value = scale( center, deadband,
+ min, max, raw_value );
} else {
scaled_value = scale( min, max, raw_value );
}
scaled_value *= factor;
+ scaled_value += offset;
+
+ // final sanity clamp
+ if ( center >= 0 ) {
+ scaled_value = clamp( -1.0, 1.0, scaled_value );
+ } else {
+ scaled_value = clamp( 0.0, 1.0, scaled_value );
+ }
// update the property tree values
for ( j = 0; j < (int)output_nodes.size(); ++j ) {
string cname = child->getName();
string name = "";
string type = "";
- vector <SGPropertyNode *> output_nodes; output_nodes.clear();
+ vector <SGPropertyNode *> output_nodes;
int row = -1;
int col = -1;
float factor = 1.0;
if ( cname == "switch" ) {
string name = "";
string type = "";
- vector <SGPropertyNode *> output_nodes; output_nodes.clear();
+ vector <SGPropertyNode *> output_nodes;
int byte_num = -1;
int right_shift = 0;
int mask = 0xff;