#include <aversive/error.h>
#include <aversive.h>
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
#include <f64.h>
+#endif
#include "angle_distance.h"
#include "robot_system.h"
IRQ_LOCK(flags);
rs->left_ext_encoder = left_ext_encoder;
rs->left_ext_encoder_param = left_ext_encoder_param;
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
rs->left_ext_gain = f64_from_double(gain);
+#else
+ rs->left_ext_gain = gain;
+#endif
IRQ_UNLOCK(flags);
}
IRQ_LOCK(flags);
rs->right_ext_encoder = right_ext_encoder;
rs->right_ext_encoder_param = right_ext_encoder_param;
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
rs->right_ext_gain = f64_from_double(gain);
+#else
+ rs->right_ext_gain = gain;
+#endif
IRQ_UNLOCK(flags);
}
/* apply gains to each wheel */
if (! (rs->flags & RS_IGNORE_EXT_GAIN )) {
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
wext.left = f64_msb_mul(f64_from_lsb(wext.left), rs->left_ext_gain);
wext.right = f64_msb_mul(f64_from_lsb(wext.right), rs->right_ext_gain);
+#else
+ double tmp;
+ tmp = wext.left;
+ tmp *= rs->left_ext_gain;
+ wext.left = tmp;
+ tmp = wext.right;
+ tmp *= rs->right_ext_gain;
+ wext.right = tmp;
+#endif
}
#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT