//
// 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$
}
+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 ) {
deadband = prop->getIntValue();
}
+ prop = child->getChild( "offset" );
+ if ( prop != NULL ) {
+ offset = prop->getFloatValue();
+ }
prop = child->getChild( "factor" );
if ( prop != NULL ) {
factor = prop->getFloatValue();
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 ) {
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;