def update(self, throttle, rudder, timestamp):
#print 'dynamics update:',throttle, rudder, timestamp
throttle = min(1.0, max(0.0, throttle))
if self.last_update is None:
delta_t = None
else:
delta_t = (timestamp - self.last_update).to_sec()
if delta_t <= 0:
delta_t = None
self.last_update = timestamp
target_rpm = self.model['idle_rpm'] + throttle * (
self.model['max_rpm'] - self.model['idle_rpm'])
if (target_rpm != self.rpm):
if delta_t is not None:
rcr = (target_rpm - self.rpm) / delta_t
if abs(rcr) > self.model['max_rpm_change_rate']:
if rcr < 0:
rcr = -self.model['max_rpm_change_rate']
else:
rcr = self.model['max_rpm_change_rate']
self.rpm += rcr * delta_t
prop_rpm = self.model['prop_ratio'] * self.rpm
prop_speed = prop_rpm / self.model['prop_pitch']
if prop_speed > 0:
rudder_speed = max(math.sqrt(prop_speed), self.speed)
else:
rudder_speed = self.speed
thrust = self.prop_coefficient * (prop_speed**2 - self.speed**2)
thrust = random.gauss(thrust, thrust * self.jitters['thrust'])
rudder_angle = rudder * self.model['max_rudder_angle']
rudder_angle += random.gauss(0.0, self.jitters['rudder'])
rudder_rads = math.radians(rudder_angle)
thrust_fwd = thrust * math.cos(rudder_rads)
rudder_speed_yaw = rudder_speed * math.sin(rudder_rads)
yaw_rate = self.model[
'rudder_coefficient'] * rudder_speed_yaw / self.model[
'rudder_distance']
if delta_t is not None:
self.heading += yaw_rate * delta_t
self.heading = math.fmod(self.heading, math.radians(360))
if self.heading < 0:
self.heading += math.radians(360)
drag = self.speed**3 * self.drag_coefficient
drag = random.gauss(drag, drag * self.jitters['drag'])
a = (thrust_fwd - drag) / self.model['mass']
if delta_t is not None:
self.speed += a * delta_t
if self.speed > 0:
(prop_rpm / self.model['prop_pitch']) / self.speed
if delta_t is not None:
last_lat = self.latitude
last_long = self.longitude
delta = self.speed * delta_t
self.longitude, self.latitude = geodesic.direct(
self.longitude, self.latitude, self.heading, delta)
if self.environment is not None:
c = self.environment.getCurrent(self.latitude, self.longitude)
if c is not None:
current_speed = random.gauss(
c['speed'], c['speed'] * self.jitters['current_speed'])
current_direction = math.radians(
c['direction']) + random.gauss(
0.0, self.jitters['current_direction'])
self.longitude, self.latitude = geodesic.direct(
self.longitude, self.latitude, current_direction,
current_speed * delta_t)
self.cog, self.sog = geodesic.inverse(last_long, last_lat,
self.longitude,
self.latitude)
self.sog /= delta_t
def update(self, throttle, rudder, timestamp):
throttle = min(1.0, max(0.0, throttle))
if self.last_update is None:
delta_t = None
else:
#delta_t = (timestamp-self.last_update).to_sec()
delta_t = timestamp - self.last_update
# ADDED FOR DEBUGGING
#print("last_update: %0.2f" % self.last_update)
self.last_update = timestamp
target_rpm = self.model['idle_rpm'] + throttle * (
self.model['max_rpm'] - self.model['idle_rpm'])
if (target_rpm != self.rpm):
if delta_t is not None:
rcr = (target_rpm - self.rpm) / delta_t
if abs(rcr) > self.model['max_rpm_change_rate']:
if rcr < 0:
rcr = -self.model['max_rpm_change_rate']
else:
rcr = self.model['max_rpm_change_rate']
self.rpm += rcr * delta_t
prop_rpm = self.model['prop_ratio'] * self.rpm
prop_speed = prop_rpm / self.model['prop_pitch']
rudder_speed = max(math.sqrt(prop_speed), self.speed)
thrust = self.prop_coefficient * (prop_speed**2 - self.speed**2)
thrust = random.gauss(thrust, thrust * 0.1)
rudder_angle = rudder * self.model['max_rudder_angle']
rudder_angle += random.gauss(0.0, 0.25)
rudder_rads = math.radians(rudder_angle)
thrust_fwd = thrust * math.cos(rudder_rads)
rudder_speed_yaw = rudder_speed * math.sin(rudder_rads)
yaw_rate = self.model[
'rudder_coefficient'] * rudder_speed_yaw / self.model[
'rudder_distance']
if delta_t is not None:
self.heading += yaw_rate * delta_t
self.heading = math.fmod(self.heading, math.radians(360))
if self.heading < 0:
self.heading += math.radians(360)
drag = self.speed**3 * self.drag_coefficient
drag = random.gauss(drag, drag * 0.1)
a = (thrust_fwd - drag) / self.model['mass']
if delta_t is not None:
self.speed += a * delta_t
if self.speed > 0:
(prop_rpm / self.model['prop_pitch']) / self.speed
if delta_t is not None:
delta = self.speed * delta_t
self.longitude, self.latitude = geodesic.direct(
self.longitude, self.latitude, self.heading, delta)
#ADDED - CM -
#ADDED FOR DEBUGGING
#print("delta: %0.3f" % (delta))
#print("delta_t: %0.3f , speed: %0.2f" % (delta_t,self.speed))
self.x = self.x + delta * math.sin(self.heading)
self.y = self.y + delta * math.cos(self.heading)