From 45d9f8ecc27870a923acbc7a503870ccb8281cb6 Mon Sep 17 00:00:00 2001
From: Greg Hogan <gregjhogan@gmail.com>
Date: Wed, 13 Oct 2021 20:03:15 -0700
Subject: [PATCH] ModelDataRaw struct with fixed size arrays (#22544)

* convert pose

* fix indexes
old-commit-hash: 21e58f9f4a43e0455ed038cf5e16302803abdf0e
---
 selfdrive/modeld/models/driving.cc | 10 +++++-----
 selfdrive/modeld/models/driving.h  |  9 ++++++++-
 2 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc
index 94b0e90fe3..603dd99210 100644
--- a/selfdrive/modeld/models/driving.cc
+++ b/selfdrive/modeld/models/driving.cc
@@ -118,7 +118,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh
   net_outputs.lead = &s->output[LEAD_IDX];
   net_outputs.lead_prob = &s->output[LEAD_PROB_IDX];
   net_outputs.meta = &s->output[DESIRE_STATE_IDX];
-  net_outputs.pose = &s->output[POSE_IDX];
+  net_outputs.pose = (ModelDataRawPose*)&s->output[POSE_IDX];
   return net_outputs;
 }
 
@@ -361,11 +361,11 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_droppe
   float rot_std_arr[3];
 
   for (int i =0; i < 3; i++) {
-    trans_arr[i] = net_outputs.pose[i];
-    trans_std_arr[i] = exp(net_outputs.pose[6 + i]);
+    trans_arr[i] = net_outputs.pose->trans_arr[i];
+    trans_std_arr[i] = exp(net_outputs.pose->trans_std_arr[i]);
 
-    rot_arr[i] = net_outputs.pose[3 + i];
-    rot_std_arr[i] = exp(net_outputs.pose[9 + i]);
+    rot_arr[i] = net_outputs.pose->rot_arr[i];
+    rot_std_arr[i] = exp(net_outputs.pose->rot_std_arr[i]);
   }
 
   MessageBuilder msg;
diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h
index 0840483eb5..d5fe5e8e33 100644
--- a/selfdrive/modeld/models/driving.h
+++ b/selfdrive/modeld/models/driving.h
@@ -18,6 +18,13 @@ constexpr int DESIRE_LEN = 8;
 constexpr int TRAFFIC_CONVENTION_LEN = 2;
 constexpr int MODEL_FREQ = 20;
 
+struct ModelDataRawPose {
+  float trans_arr[3];
+  float rot_arr[3];
+  float trans_std_arr[3];
+  float rot_std_arr[3];
+};
+
 struct ModelDataRaw {
   float *plan;
   float *lane_lines;
@@ -28,7 +35,7 @@ struct ModelDataRaw {
   float *desire_state;
   float *meta;
   float *desire_pred;
-  float *pose;
+  ModelDataRawPose *pose;
 };
 
 typedef struct ModelState {