+ def execute_dyn_reconf(self, config, level):
+ self.bClipRangeSensor = config["range_sensor_clip"]
+ self.range_sensor_max = config["range_sensor_max"]
+ self.odom_covar_xy = config["odom_covar_xy"]
+ self.odom_covar_angle = config["odom_covar_angle"]
+ self.rollover_protect = config["rollover_protect"]
+ self.rollover_protect_limit = config["rollover_protect_limit"]
+ self.rollover_protect_pwm = config["rollover_protect_pwm"]
+
+ return config
+
+ def imuReceived(self, msg):
+ if self.rollover_protect:
+ (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(msg.orientation.__getstate__())
+ if pitch > self.rollover_protect_limit*pi/180:
+ self.bMotorManual = True
+ i2c_write_reg(0x50, 0x3, struct.pack(">h", self.rollover_protect_pwm))
+ i2c_write_reg(0x50, 0x7, struct.pack(">h", self.rollover_protect_pwm))
+ elif pitch < -self.rollover_protect_limit*pi/180:
+ self.bMotorManual = True
+ i2c_write_reg(0x50, 0x1, struct.pack(">h", -self.rollover_protect_pwm))
+ i2c_write_reg(0x50, 0x5, struct.pack(">h", -self.rollover_protect_pwm))
+ elif self.bMotorManual:
+ i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", 0, 0, 0, 0))
+ self.bMotorManual = False
+