diff --git a/display.py b/display.py index 22d557b..d9c8cb4 100644 --- a/display.py +++ b/display.py @@ -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 @@ -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) @@ -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) diff --git a/ekf.py b/ekf.py index daf7391..b58ee11 100644 --- a/ekf.py +++ b/ekf.py @@ -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, @@ -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 diff --git a/shumai.py b/shumai.py index 23f3344..a29decc 100644 --- a/shumai.py +++ b/shumai.py @@ -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") @@ -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), @@ -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()) diff --git a/vgo.py b/vgo.py index 1193f8c..6ad68c5 100644 --- a/vgo.py +++ b/vgo.py @@ -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