Map based speed fixes (commaai#137)

* fixed map based speed slowed you down on roads without valid map data, even if map data was disabled
* fixed map based speed miscalculation when speed limit was higher than max cruise speed (car drove slower than any of those three values)
* fleet speed no longer affects road sections without a speed limit like the autobahn, fixing again unnecessary slow downs

selfdrive/car/tesla/ACC_module.py

@@ -1,4 +1,3 @@
-from selfdrive.services import service_list
 from selfdrive.car.tesla.values import CruiseButtons, CruiseState
 from selfdrive.config import Conversions as CV
 import selfdrive.messaging as messaging
@@ -182,22 +181,26 @@ def _update_max_acc_speed(self, CS):
     # Clip ACC speed between 0 and 170 KPH.
     self.acc_speed_kph = min(self.acc_speed_kph, 170)
     self.acc_speed_kph = max(self.acc_speed_kph, 0)
-
-  def max_v_by_speed_limit(self, acc_set_speed_ms, CS):
-    # if more than 10 kph / 2.78 ms, consider we have speed limit
-    if (CS.maxdrivespeed > 0)  and CS.useTeslaMapData and (CS.mapAwareSpeed or (CS.baseMapSpeedLimitMPS <2.7)):
-      #do we know the based speed limit?
-      sl1 = 0.
-      if CS.baseMapSpeedLimitMPS >= 2.7:
-        #computer adjusted maxdrive based on set speed
-        sl1 = min (acc_set_speed_ms *  CS.maxdrivespeed / CS.baseMapSpeedLimitMPS, acc_set_speed_ms)
-        sl1 = self.maxsuggestedspeed_avg.add(sl1)
+
+  def max_v_by_map_data(self, acc_set_speed_ms, CS):
+    if CS.mapAwareSpeed and self._has_valid_map_speed(CS):
+      # if set speed is greater than the speed limit, apply a relative offset to map speed
+      if CS.rampType == 0 and acc_set_speed_ms > CS.baseMapSpeedLimitMPS > CS.map_suggested_speed:
+        sl1 = self.maxsuggestedspeed_avg.add(acc_set_speed_ms * CS.map_suggested_speed / CS.baseMapSpeedLimitMPS)
       else:
-        sl1 = self.maxsuggestedspeed_avg.add(CS.maxdrivespeed)
+        sl1 = self.maxsuggestedspeed_avg.add(CS.map_suggested_speed)
       return min(acc_set_speed_ms, sl1)
     else:
       return acc_set_speed_ms
 
+  def _has_valid_map_speed(self, CS):
+    if CS.map_suggested_speed == 0:
+      return False
+    if CS.baseMapSpeedLimitMPS == 0: # no or unknown speed limit
+      if CS.rampType == 0 and CS.map_suggested_speed >= 17: # more than 61 kph / 38 mph, means we may be on a road without speed limit
+        return False
+    return True
+
   # Decide which cruise control buttons to simluate to get the car to the
   # desired speed.
   def update_acc(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_kph, speed_limit_valid, set_speed_limit_active, speed_limit_offset):
@@ -277,7 +280,7 @@ def _calc_follow_button(self, CS, lead_car,speed_limit_kph, speed_limit_valid, s
     # Relative velocity between the lead car and our set cruise speed.
     future_vrel_kph = lead_speed_kph - CS.v_cruise_actual
     # How much we can accelerate without exceeding the max allowed speed.
-    max_acc_speed_kph = self.max_v_by_speed_limit(self.acc_speed_kph * CV.KPH_TO_MS, CS) * CV.MS_TO_KPH
+    max_acc_speed_kph = self.max_v_by_map_data(self.acc_speed_kph * CV.KPH_TO_MS, CS) * CV.MS_TO_KPH
     available_speed_kph = max_acc_speed_kph - CS.v_cruise_actual
     half_press_kph, full_press_kph = self._get_cc_units_kph(CS.imperial_speed_units)
     # button to issue