Skip to content

Commit

Permalink
Added categories to the display, e.g. "performance" and "sensor"
Browse files Browse the repository at this point in the history
  • Loading branch information
@timtrueman
timtrueman committed Apr 10, 2010
1 parent f4b9afd commit 8757beb
Show file tree
Hide file tree
Showing 4 changed files with 62 additions and 51 deletions.
73 changes: 41 additions & 32 deletions display.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,7 @@ def __new__(cls, *args, **kwargs):
return cls._instance

def __init__(self):
self.scalars = {}
self.matrices = {}
self.categories = {}
try:
import curses
self.curses_available = True
Expand All @@ -26,19 +25,21 @@ def __del__(self):
if self.curses_available:
curses.endwin()

def register_scalar(self, label, scalar):
self.scalars[label] = scalar
def register_scalar(self, label, scalar, category="Internal states"):
self.categories.setdefault(category, {"scalars":{},"matrices":{}})
self.categories[category]['scalars'][label] = scalar

def register_scalars(self, scalars):
def register_scalars(self, scalars, category="Internal states"):
for label, scalar in scalars.items():
self.register_scalar(label, scalar)
self.register_scalar(label, scalar, category)

def register_matrix(self, label, matrix):
self.matrices[label] = matrix
def register_matrix(self, label, matrix, category="Internal states"):
self.categories.setdefault(category, {"scalars":{},"matrices":{}})
self.categories[category]['matrices'][label] = matrix

def register_matrices(self, matrices):
def register_matrices(self, matrices, category="Internal states"):
for label, matrix in matrices.items():
self.register_matrix(label, matrix)
self.register_matrix(label, matrix, category)

def display_matrix(self, m, x, y, precision=2, title=None):
a, b = self.get_matrix_display_size(m, precision=precision)
Expand Down Expand Up @@ -68,35 +69,43 @@ def get_matrix_display_size(self, m, precision=2, title=True):
def display_state(self, precision):
self.screen.erase()
rows, cols = self.screen.getmaxyx()
scalar_count = len(self.scalars)
self.screen.addstr(0, 0, "Shumai: the Extended Kalman Filter for aircraft")
i = 1
x, y = 2, 0
for s in self.scalars.keys():
if y + precision + len(s) > cols:
x += 1
y = 0
if x < rows and y + precision + len(s) < cols:
self.screen.addstr(x, y, "%s: %+0.*f" % (s, precision, self.scalars[s]))
y += 20 + precision
i += 1
x, y = x + 2, 0
maxheight = 0
for m in self.matrices.keys():
matrix = self.matrices[m]
a, b = self.get_matrix_display_size(matrix, precision=precision)
if a > maxheight:
maxheight = a
if b + y > cols-10:
y = 0
for category in sorted(self.categories.keys(), key=str.lower):
self.screen.addstr(x, y, "%s:" % category)
x += 2
for s in sorted(self.categories[category]['scalars'].keys(), key=str.lower):
if y + precision + len(s) > cols:
x += 1
y = 0
if x < rows and y + precision + len(s) < cols:
self.screen.addstr(x, y, "%s: %+0.*f" % (s, precision, self.categories[category]['scalars'][s]))
y += 20 + precision
i += 1
if len(self.categories[category]['scalars']) > 0:
x, y = x + 2, 0
maxheight = 0
for m in sorted(self.categories[category]['matrices'].keys(), key=str.lower):
matrix = self.categories[category]['matrices'][m]
a, b = self.get_matrix_display_size(matrix, precision=precision)
if a > maxheight:
maxheight = a
if b + y > cols-10:
y = 0
x += maxheight + 1
maxheight = 0
c, d = self.display_matrix(matrix, x, y, precision=precision, title=m)
y += b + 3
if len(self.categories[category]['matrices']) > 0:
x += maxheight + 2
else:
x += maxheight + 1
maxheight = 0
c, d = self.display_matrix(matrix, x, y, precision=precision, title=m)
y += b + 3
y = 0
# point the cursor in the bottom right corner so it doesn't hide anything
self.screen.move(rows-1, cols-1)

def draw(self, precision=5):
def draw(self, precision=4):
self.display_state(precision=precision)
rows, cols = self.screen.getmaxyx()
self.screen.move(rows-1, cols-1)
Expand Down
21 changes: 11 additions & 10 deletions ekf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,19 +145,17 @@ def update_covariance_matrix(self):
self.Pz = (self.I - self.Lz * self.Cz) * self.Pz

def register_states(self):
scalars = {"p (rad)":self.p,
"q (rad)":self.q,
"r (rad)":self.r,
scalars = {"p (deg)":degrees(self.p),
"q (deg)":degrees(self.q),
"r (deg)":degrees(self.r),
"ax":self.ax,
"ay":self.ay,
"az":self.az,
"Acc mag":self.acceleration_magnitude,
"Acc wght":self.acceleration_weight,
# "Acc mag":self.acceleration_magnitude,
# "Acc wght":self.acceleration_weight,
"axhat":self.axhat,
"ayhat":self.ayhat,
"azhat":self.azhat,
"Phi (deg)":degrees(self.phi),
"Theta (deg)":degrees(self.theta),}
"azhat":self.azhat,}
matrices = {"A":self.A,
"Q":self.Q,
"Px":self.Px,
Expand All @@ -169,8 +167,11 @@ def register_states(self):
"Lx":self.Lx,
"Ly":self.Ly,
"Lz":self.Lz,}
display.register_scalars(scalars)
display.register_matrices(matrices)
estimates = {"Phi (deg)":degrees(self.phi),
"Theta (deg)":degrees(self.theta),}
display.register_scalars(scalars, "Sensors")
display.register_scalars(estimates, "Estimates")
display.register_matrices(matrices, "Internal states")

def estimate_roll_and_pitch(self, p, q, r, Vair, ax, ay, az, dt):
self.p, self.q, self.r = p, q, r
Expand Down
17 changes: 9 additions & 8 deletions shumai.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
from gps import EmulatedXplaneGPS
from utils import get_ip_address, wrap
from truthdata import TruthData
# from autopilot import Autopilot
from autopilot import Autopilot

logging.config.fileConfig("logging.conf")
logger = logging.getLogger("shumai")
Expand Down Expand Up @@ -90,11 +90,11 @@ def loop(self):
display.register_scalars({"gps_lat": gps_data['latitude'],
"gps_lon": gps_data['longitude'],
"gps_alt": gps_data['altitude'],
"gps_sog": gps_data['speed_over_ground'],})
"gps_sog": gps_data['speed_over_ground'],}, "Sensors")
altitude = self.altitude_observer.estimate(theta, Vair, gps_data['altitude'], TD.DT)
display.register_scalars({"alt_est": altitude})
display.register_scalars({"alt_est": altitude}, "Estimates")
wind_direction, wind_velocity = self.wind_observer.estimate(theta, psi, Vair, gps_data['speed_over_ground'], gps_data['course_over_ground'], TD.DT)
display.register_scalars({"wind_direction": wind_direction, "wind_velocity": wind_velocity, "COG": gps_data['course_over_ground']})
display.register_scalars({"wind_direction": wind_direction, "wind_velocity": wind_velocity, "COG": gps_data['course_over_ground']}, "Estimates")
#altitude = self.estimate(theta, Vair, gps_data['altitude'], DT)
return {
"roll": degrees(phi),
Expand Down Expand Up @@ -137,18 +137,19 @@ def datagramReceived(self, data, (host, port)):
TD.LONGITUDE = unpack_from(fmt, data, 9+180+4)[0]
TD.ALTITUDE = unpack_from(fmt, data, 9+180+20)[0]
TD.SPEEDOVERGROUND = unpack_from(fmt, data, 9+12)[0]
display.register_scalars({"lat":TD.LATITUDE,"lon":TD.LONGITUDE,"alt":TD.ALTITUDE,"sog":TD.SPEEDOVERGROUND})
display.register_scalars({"lat":TD.LATITUDE,"lon":TD.LONGITUDE,"alt":TD.ALTITUDE,"sog":TD.SPEEDOVERGROUND}, "Sensors")
self.generate_virtual_magnetometer_readings(TD.ROLL,TD.PITCH,TD.HEADING)
display.register_scalars({"bx":TD.BX,"by":TD.BY,"bz":TD.BZ,"true heading":degrees(TD.HEADING)})
display.register_scalars({"bx":TD.BX,"by":TD.BY,"bz":TD.BZ,"true heading":degrees(TD.HEADING)}, "Sensors")
logger.debug("Vair %0.1f, accelerometers (%0.2f, %0.2f, %0.2f), gyros (%0.2f, %0.2f, %0.2f)" % (TD.AIRSPEED, TD.AX, TD.AY, TD.AZ, TD.P, TD.Q, TD.R))
current_state = self.ekf.loop()
display.register_scalars({"Psi error":current_state['yaw']-degrees(TD.HEADING)})
display.register_scalars({"Psi error":current_state['yaw']-degrees(TD.HEADING)}, "Performance")
if display.curses_available is True:
display.draw()
else:
sys.stdout.write("%sRoll = %f, pitch = %f " % (chr(13), current_state['roll'], current_state['pitch']))
sys.stdout.flush()
FOUT.writerow([degrees(TD.ROLL), degrees(TD.PITCH), current_state['roll'], current_state['pitch'], current_state['roll'] - degrees(TD.ROLL), current_state['pitch'] - degrees(TD.PITCH)])
#FOUT.writerow([degrees(TD.ROLL), degrees(TD.PITCH), current_state['roll'], current_state['pitch'], current_state['roll'] - degrees(TD.ROLL), current_state['pitch'] - degrees(TD.PITCH)])
display.register_scalars({"Phi error":current_state['roll']-degrees(TD.ROLL), "Theta error":current_state['pitch'] - degrees(TD.PITCH)}, "Performance")
self.autopilot.heading_hold()
self.sendJoystick((self.autopilot.roll_hold(), self.autopilot.pitch_hold()), 0)
self.sendThrottle(self.autopilot.throttle())
Expand Down
2 changes: 1 addition & 1 deletion vgo.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,5 +54,5 @@ def estimate_heading(self, bx, by, bz, phi, theta, q, r, dt):
psi_error = self.psi_estimate - psi_measured
self.psi_estimate += self.k_psi * psi_error
logger.info("Heading %f" % self.psi_estimate)
display.register_scalars({"Psi":degrees(self.psi_estimate),"psi_dot":degrees(psi_dot)})
display.register_scalars({"Psi":degrees(self.psi_estimate)}, "Estimates")
return self.psi_estimate

0 comments on commit 8757beb

Please sign in to comment.