Merge pull request commaai#374 from ShaneSmiskol/release2

Add drive data collection for a longitudinal model

selfdrive/car/toyota/carstate.py

@@ -59,6 +59,7 @@ def get_can_parser(CP):
     ("SPORT_ON", "GEAR_PACKET", 0),
     ("ECON_ON", "GEAR_PACKET", 0),
     ("BRAKE_PRESSED", "BRAKE_MODULE", 0),
+    ("BRAKE_PRESSURE", "BRAKE_MODULE", 0),
     ("GAS_PEDAL", "GAS_PEDAL", 0),
     ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
     ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
@@ -385,7 +386,7 @@ def update(self, cp, cp_cam):
 
     self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
     if self.CP.enableGasInterceptor:
-      self.pedal_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
+      self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
     else:
       self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
     self.car_gas = self.pedal_gas
@@ -519,7 +520,7 @@ def update(self, cp, cp_cam):
     # we could use the override bit from dbc, but it's triggered at too high torque values
     self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
 
-    self.user_brake = 0
+    self.user_brake = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSURE']
     if self.CP.carFingerprint == CAR.LEXUS_IS:
       self.pcm_acc_status = cp.vl["PCM_CRUISE_3"]['CRUISE_STATE']
       self.v_cruise_pcm = cp.vl["PCM_CRUISE_3"]['SET_SPEED']

selfdrive/controls/lib/long_mpc.py

@@ -31,7 +31,8 @@ def __init__(self, mpc_id, live_longitudinal_mpc):
     self.v_lead = None
     self.x_lead = None
     self.phantom = Phantom(timeout=True, do_sshd_mod=True)
-
+    self.df_data = []
+    self.df_frame = 0
 
     self.last_cloudlog_t = 0.0
 
@@ -199,6 +200,10 @@ def process_phantom(self, lead):
     return x_lead, v_lead
 
   def update(self, CS, lead, v_cruise_setpoint):
+    v_ego = CS.carState.vEgo
+    a_ego = CS.carState.aEgo
+    gas = CS.carState.gas
+    brake = CS.carState.brake
     self.car_state = CS.carState
     self.v_ego = CS.carState.vEgo
 
@@ -226,6 +231,7 @@ def update(self, CS, lead, v_cruise_setpoint):
       self.cur_state[0].v_l = v_lead
     else:
       if lead is not None and lead.status:
+        self.df_frame += 1
         x_lead = lead.dRel
         v_lead = max(0.0, lead.vLead)
         a_lead = lead.aLeadK
@@ -234,6 +240,17 @@ def update(self, CS, lead, v_cruise_setpoint):
           v_lead = 0.0
           a_lead = 0.0
 
+        if self.mpc_id == 1 and not CS.carState.cruiseState.enabled:
+          self.df_data.append([v_ego, a_ego, v_lead, x_lead, a_lead, gas, brake, time.time()])
+          if self.df_frame >= 200:  # every 5 seconds, write to file
+            try:
+              with open("/data/openpilot/selfdrive/df/df-data", "a") as f:
+                f.write("\n".join([str(i) for i in self.df_data]) + "\n")
+              self.df_data = []
+              self.df_frame = 0
+            except Exception,e:
+              pass
+
         self.v_lead = v_lead
         self.x_lead = x_lead
         self.a_lead_tau = lead.aLeadTau

selfdrive/df/df_uploader.py

@@ -0,0 +1,44 @@
+import ftplib
+import json
+import string
+import random
+import os
+from common.params import Params
+
+def upload_data():
+  filepath = "/data/openpilot/selfdrive/df/df-data"
+  if os.path.isfile(filepath):
+    try:
+      username = ''.join([random.choice(string.lowercase+string.uppercase+string.digits) for i in range(15)])
+      try:
+        with open("/data/data/ai.comma.plus.offroad/files/persistStore/persist-auth", "r") as f:
+          auth = json.loads(f.read())
+        auth = json.loads(auth['commaUser'])
+        if auth:
+          username = str(auth['username'])
+      except:
+        pass
+
+      params = Params()
+      car = params.get('CachedFingerprint')
+      if car is not None:
+        car = json.loads(car)
+        username+="-{}".format(car[0])
+
+      filename = "df-data.{}".format(random.randint(1,99999))
+
+      ftp = ftplib.FTP("smiskol.com")
+      ftp.login("eon", "87pYEYF4vFpwvgXU")
+      with open(filepath, "rb") as f:
+        try:
+          ftp.mkd("/{}".format(username))
+        except:
+          pass
+        ftp.storbinary("STOR /{}/{}".format(username, filename), f)
+      ftp.quit()
+      os.remove(filepath)
+      return True
+    except:
+      return False
+  else:
+    return False

selfdrive/loggerd/uploader.py

@@ -15,6 +15,7 @@
 from collections import Counter
 from selfdrive.swaglog import cloudlog
 from selfdrive.loggerd.config import ROOT
+from selfdrive.df import df_uploader
 
 from common.params import Params
 from common.api import api_get
@@ -267,12 +268,29 @@ def uploader_fn(exit_event):
   uploader = Uploader(dongle_id, access_token, ROOT)
 
   backoff = 0.1
+
+  try:
+    last_df_size = os.path.getsize("/data/openpilot/selfdrive/df/df-data")
+  except:
+    last_df_size = None
   while True:
     allow_cellular = (params.get("IsUploadVideoOverCellularEnabled") != "0")
     on_hotspot = is_on_hotspot()
     on_wifi = is_on_wifi()
     should_upload = allow_cellular or (on_wifi and not on_hotspot)
 
+    try:
+      if last_df_size == os.path.getsize("/data/openpilot/selfdrive/df/df-data"):
+        if on_wifi and not on_hotspot:
+          df_uploader.upload_data()
+    except:
+      pass
+
+    try:
+      last_df_size = os.path.getsize("/data/openpilot/selfdrive/df/df-data")
+    except:
+      last_df_size = None
+
     if exit_event.is_set():
       return