Display steering angle, speed & cones in AMI

TouchGFX/gui/src/ami_screen/AMIPresenter.cpp

@@ -1,6 +1,7 @@
 #include <gui/ami_screen/AMIPresenter.hpp>
 #include <gui/ami_screen/AMIView.hpp>
 
+#include "stm32h7xx_hal.h"
 #include "vehicle_state.h"
 
 AMIPresenter::AMIPresenter(AMIView &v) : view(v) {}
@@ -11,4 +12,19 @@ void AMIPresenter::deactivate() {}
 
 void AMIPresenter::vehicleStateUpdated() {
   view.setMission(vehicle_state.active_mission);
+  view.updateDataFields();
+#ifndef SIMULATOR
+  view.setJetsonTimeout(HAL_GetTick() - vehicle_state.last_jetson_msg > 500);
+  view.setEPSCTimeout(HAL_GetTick() - vehicle_state.last_epsc_msg > 500);
+#endif
+
+  size_t cone_count = 0;
+  for (; cone_count < NUM_CONES; cone_count++) {
+    // A cone position of 0xFF/0xFF indicates that there are no more cones
+    if (vehicle_state.cone_pos[cone_count].x == 0xFF &&
+        vehicle_state.cone_pos[cone_count].y == 0xFF) {
+      break;
+    }
+  }
+  view.setConePositions(vehicle_state.cone_pos, cone_count);
 }

TouchGFX/gui/src/ami_screen/AMIView.cpp

@@ -1,10 +1,28 @@
+#include "gui_generated/ami_screen/AMIViewBase.hpp"
 #include "stw_defines.h"
 #include "texts/TextKeysAndLanguages.hpp"
 #include "touchgfx/Color.hpp"
 #include "touchgfx/TypedText.hpp"
+#include "touchgfx/Unicode.hpp"
+#include "touchgfx/widgets/TextAreaWithWildcard.hpp"
+#include "vehicle_state.h"
 #include <gui/ami_screen/AMIView.hpp>
 
-AMIView::AMIView() {}
+AMIDataField::AMIDataField(touchgfx::TextAreaWithOneWildcard &textArea,
+                           const char *fmt)
+    : fmt(fmt), textArea(textArea) {}
+
+void AMIDataField::setValue(float value) {
+  Unicode::snprintfFloat(buffer, sizeof(buffer) / sizeof(*buffer), fmt, value);
+  textArea.setWildcard(buffer);
+  textArea.invalidate();
+}
+
+AMIView::AMIView()
+    : AMIViewBase(), desiredAngleField(desiredAngle, "%5.2f"),
+      measuredAngleField(measuredAngle, "%5.2f"),
+      desiredSpeedField(desiredSpeed, "%5.1f"),
+      measuredSpeedField(measuredSpeed, "%5.1f") {}
 
 void AMIView::setupScreen() { AMIViewBase::setupScreen(); }
 
@@ -34,8 +52,43 @@ void AMIView::setMission(Mission mission) {
   case MISSION_NONE:
   default:
     currentMission.setTypedText(TypedText(T_INVALID_HUGE));
-    currentMission.setColor(touchgfx::Color::getColorFromRGB(0xFF, 0, 0));
     break;
   }
   currentMission.invalidate();
 }
+
+void AMIView::updateDataFields() {
+  desiredAngleField.setValue(vehicle_state.desired_angle);
+  measuredAngleField.setValue(vehicle_state.measured_angle);
+  desiredSpeedField.setValue(vehicle_state.desired_speed);
+  measuredSpeedField.setValue(vehicle_state.speed);
+}
+
+void AMIView::setJetsonTimeout(bool timeout) {
+  if (timeout) {
+    desiredAngle.setColor(touchgfx::Color::getColorFromRGB(0xFF, 0, 0));
+  } else {
+    desiredAngle.setColor(touchgfx::Color::getColorFromRGB(0xFF, 0xFF, 0xFF));
+  }
+}
+
+void AMIView::setEPSCTimeout(bool timeout) {
+  if (timeout) {
+    measuredAngle.setColor(touchgfx::Color::getColorFromRGB(0xFF, 0, 0));
+  } else {
+    measuredAngle.setColor(touchgfx::Color::getColorFromRGB(0xFF, 0xFF, 0xFF));
+  }
+}
+
+void AMIView::setConePositions(ConePosition *cone_positions, size_t count) {
+  for (size_t i = 0; i < count; i++) {
+    cones[i].setX(cone_positions[i].x);
+    cones[i].setY(cone_positions[i].y);
+    cones[i].setVisible(true);
+    cones[i].invalidate();
+  }
+  for (size_t i = count; i < NUM_CONES; i++) {
+    cones[i].setVisible(false);
+    cones[i].invalidate();
+  }
+}