raw pred api (#2611)

* send both

* update cereal

* ptr type

* this is fine

* clean up

* v1 and v2

* header

* update cereal

* cereal

selfdrive/modeld/dmonitoringmodeld.cc

@@ -62,7 +62,8 @@ int main(int argc, char **argv) {
       double t2 = millis_since_boot();
 
       // send dm packet
-      dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0);
+      const float* raw_pred_ptr = send_raw_pred ? (const float *)dmonitoringmodel.output : nullptr;
+      dmonitoring_publish(pm, extra.frame_id, res, raw_pred_ptr, (t2-t1)/1000.0);
 
       LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
       last = t1;

selfdrive/modeld/modeld.cc

@@ -201,8 +201,9 @@ int main(int argc, char **argv) {
         if (run_count < 10) frames_dropped = 0;  // let frame drops warm up
         float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
 
-        model_publish(pm, extra.frame_id, frame_id,  vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time);
-        model_publish_v2(pm, extra.frame_id, frame_id,  vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time);
+        const float* raw_pred_ptr = send_raw_pred ? (const float *)model.output : nullptr;
+        model_publish(pm, extra.frame_id, frame_id,  vipc_dropped_frames, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time);
+        model_publish_v2(pm, extra.frame_id, frame_id,  vipc_dropped_frames, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time);
         posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof);
 
         LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio);

selfdrive/modeld/models/commonmodel.h

@@ -9,6 +9,7 @@
 #endif
 
 #include <float.h>
+#include <stdlib.h>
 #include "common/mat.h"
 #include "transforms/transform.h"
 #include "transforms/loadyuv.h"
@@ -17,6 +18,8 @@
 extern "C" {
 #endif
 
+const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL;
+
 void softmax(const float* input, float* output, size_t len);
 float softplus(float input);
 float sigmoid(float input);

selfdrive/modeld/models/dmonitoring.cc

@@ -181,7 +181,7 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
   return ret;
 }
 
-void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time){
+void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, const float* raw_pred, float execution_time){
   // make msg
   MessageBuilder msg;
   auto framed = msg.initEvent().initDriverState();
@@ -198,6 +198,9 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu
   framed.setLeftBlinkProb(res.left_blink_prob);
   framed.setRightBlinkProb(res.right_blink_prob);
   framed.setSgProb(res.sg_prob);
+  if (send_raw_pred) {
+    framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, OUTPUT_SIZE*sizeof(float)));
+  }
 
   pm.send("driverState", msg);
 }

selfdrive/modeld/models/dmonitoring.h

@@ -37,7 +37,7 @@ typedef struct DMonitoringModelState {
 
 void dmonitoring_init(DMonitoringModelState* s);
 DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height);
-void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time);
+void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, const float* raw_pred, float execution_time);
 void dmonitoring_free(DMonitoringModelState* s);
 
 #ifdef __cplusplus

selfdrive/modeld/models/driving.cc

@@ -295,7 +295,7 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
 
 void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
                      uint32_t vipc_dropped_frames, float frame_drop,
-                     const ModelDataRaw &net_outputs, uint64_t timestamp_eof,
+                     const ModelDataRaw &net_outputs, const float* raw_pred, uint64_t timestamp_eof,
                      float model_execution_time) {
   // make msg
   MessageBuilder msg;
@@ -306,6 +306,9 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
   framed.setFrameDropPerc(frame_drop * 100);
   framed.setTimestampEof(timestamp_eof);
   framed.setModelExecutionTime(model_execution_time);
+  if (send_raw_pred) {
+    framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, (OUTPUT_SIZE+TEMPORAL_SIZE)*sizeof(float)));
+  }
 
   // plan
   int plan_mhp_max_idx = 0;
@@ -371,7 +374,7 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
 
 void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
                    uint32_t vipc_dropped_frames, float frame_drop,
-                   const ModelDataRaw &net_outputs, uint64_t timestamp_eof,
+                   const ModelDataRaw &net_outputs, const float* raw_pred, uint64_t timestamp_eof,
                    float model_execution_time) {
 
   uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
@@ -382,6 +385,9 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
   framed.setFrameDropPerc(frame_drop * 100);
   framed.setTimestampEof(timestamp_eof);
   framed.setModelExecutionTime(model_execution_time);
+  if (send_raw_pred) {
+    framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, (OUTPUT_SIZE+TEMPORAL_SIZE)*sizeof(float)));
+  }
 
   // Find the distribution that corresponds to the most probable plan
   int plan_mhp_max_idx = 0;

selfdrive/modeld/models/driving.h

@@ -77,10 +77,10 @@ void model_free(ModelState* s);
 void poly_fit(float *in_pts, float *in_stds, float *out);
 
 void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                   uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data,
+                   uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, const float* raw_pred,
                    uint64_t timestamp_eof, float model_execution_time);
 void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                   uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data,
+                   uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, const float* raw_pred,
                    uint64_t timestamp_eof, float model_execution_time);
 void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
                      uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data,