def update_OptFlowRad(self, optflow):
ts = optflow.header.stamp
r = optflow.distance
print 'opt flow distance is ', r
# small measurements may be invalid
if r < 0.32:
return
# sonar may give wonky measurements when uav isn't level
if not model.vehicle_is_level(self.x):
return
# handle timing issues
if self.prop_time is not None:
dt = (ts - self.prop_time).to_sec()
if dt < -0.1:
rospy.logwarn('lidarlite message %f seconds old, skipping', -dt)
return
elif dt > 0.1:
rospy.logwarn('lidarlite message %f seconds in future, skipping', dt)
return
elif dt > 0:
(x, Sigma) = self.process(self.x, self.Sigma, self.u, dt, self.disturb_mode)
else:
(x, Sigma) = (self.x, self.Sigma)
# Now do correction
z = np.array([r])
(h,Hx,Q) = model.observation_alt_px4flow(x, self.disturb_mode, z)
(x_c, Sigma_c) = self.update(x, Sigma, z, h, Hx, Q)
(self.x, self.Sigma) = model.enforce_bounds(x_c, Sigma_c)
return
def update_IMU(self, imu):
ts = imu.header.stamp
# use UNCORRECTED rates as control input
gyro_u = np.array([imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z])
acc = np.array([imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z])
if self.flight_state == model.FLIGHT_STATE_FLIGHT:
# check for bump/disturbance
if model.accel_detect_bump(self.x, acc, 15.0):
self.disturb_mode = model.DISTURB_ACTIVE
self.bump_time = ts
rospy.loginfo('Bump detected')
else:
# if we're not nominal, but it's been a while since last bump, return to nominal
if self.disturb_mode != model.DISTURB_NOMINAL and (ts - self.bump_time).to_sec() > 0.25:
self.disturb_mode = model.DISTURB_NOMINAL
rospy.loginfo('Return to nominal')
# propogate if neccessary
self.propogate_from_imu(ts, gyro_u)
# and do correction based on accelerometer
(h, Hx, Q) = model.observation_acc_flight(self.x, self.disturb_mode)
(x_c, Sigma_c) = self.update(self.x, self.Sigma, acc, h, Hx, Q)
(self.x, self.Sigma) = model.enforce_bounds(x_c, Sigma_c)
# detect landing
if self.disturb_mode == model.DISTURB_NOMINAL and model.vehicle_is_level(self.x) and model.vehicle_is_stationary(self.x, gyro_u, acc):
if self.potential_landing_ts is None:
self.potential_landing_ts == ts
self.potential_landing_nonstationary_count = 0
elif (ts - self.potential_landing_ts) > 0.25:
rospy.loginfo('Landing detected. Entering ground mode.')
self.potential_landing_ts = None
self.flight_state = model.FLIGHT_STATE_GROUNDED
else:
# we aren't stationary, can't be landed
self.potential_landing_ts = None
elif self.flight_state == model.FLIGHT_STATE_TAKEOFF:
# just propogate imu
self.propogate_from_imu(ts, gyro_u)
# transition to flight mode after short period
if (ts - self.takeoff_ts).to_sec() > 1.0:
self.flight_state = model.FLIGHT_STATE_FLIGHT
rospy.loginfo('Entering flight mode.')
else: # we're grounded
# check for takeoff
if model.accel_detect_takeoff(self.x, acc):
rospy.loginfo('Takeoff detected.')
self.flight_state = model.FLIGHT_STATE_TAKEOFF
self.takeoff_ts = ts
# assume we start in disturbed state
self.disturb_mode = model.DISTURB_ACTIVE
self.bump_time = ts
print self.Sigma
else: # use grounded data
# if there is very little motion, we can zupt
if not model.accel_detect_bump(self.x, acc, 0.10):
rospy.loginfo('zupting')
(h, Hx, Q) = model.observation_zupt(self.x)
(x_c, Sigma_c) = self.update(self.x, self.Sigma, gyro_u, h, Hx, Q)
(self.x, self.Sigma) = model.enforce_bounds(x_c, Sigma_c)
self.prop_time = ts
# otherwise propogate grounded model
# and use grounded accelerometer model
else:
self.propogate_from_imu(ts, gyro_u)
(h, Hx, Q) = model.observation_acc_ground(self.x, self.disturb_mode)
(x_c, Sigma_c) = self.update(self.x, self.Sigma, acc, h, Hx, Q)
(self.x, self.Sigma) = model.enforce_bounds(x_c, Sigma_c)
self.prop_time = ts
return
