recipes-config/cluster-demo-simulator/files/simple_can_simulator.py

   1 #!/usr/bin/env python3
   2 # Copyright (c) 2016 Alex Bencz
   3 # Copyright (c) 2019 Konsulko Group, smurray@konsulko.com
   4 # Copyright (c) 2020 The Linux Foundation, jsmoeller@linuxfoundation.org
   5 #
   6 # Permission is hereby granted, free of charge, to any person obtaining a copy of
   7 # this software and associated documentation files (the "Software"), to deal in
   8 # the Software without restriction, including without limitation the rights to
   9 # use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
  10 # of the Software, and to permit persons to whom the Software is furnished to do
  11 # so, subject to the following conditions:
  12 #
  13 # The above copyright notice and this permission notice shall be included in all
  14 # copies or substantial portions of the Software.
  15 #
  16 # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  17 # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  18 # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  19 # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  20 # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  21 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  22 # SOFTWARE.
  23
  24 #
  25 # CANSocket from:
  26 #
  27 # https://github.com/abencz/python_socketcan/blob/master/python_socketcan_example.py
  28 #
  29
  30 import sys
  31 import socket
  32 import argparse
  33 import struct
  34 import errno
  35 import threading
  36 import time
  37
  38 class CANSocket(object):
  39     FORMAT = "<IB3x8s"
  40     FD_FORMAT = "<IB3x64s"
  41
  42     def __init__(self, interface=None):
  43         self.sock = socket.socket(socket.PF_CAN, socket.SOCK_RAW, socket.CAN_RAW)
  44         if interface is not None:
  45             self.bind(interface)
  46
  47     def bind(self, interface):
  48         self.sock.bind((interface,))
  49         self.sock.setsockopt(socket.SOL_CAN_RAW, socket.CAN_RAW_FD_FRAMES, 1)
  50
  51     def send(self, can_id, data, flags=0):
  52         can_id = can_id | flags
  53         can_pkt = struct.pack(self.FORMAT, can_id, len(data), data)
  54         self.sock.send(can_pkt)
  55
  56     def sendfd(self, can_id, data, flags=0):
  57         can_id = can_id | flags
  58         datafd = data.ljust(64, b'\x00');
  59         can_pkt = struct.pack(self.FD_FORMAT, can_id, len(datafd), datafd)
  60         self.sock.send(can_pkt)
  61
  62     def recv(self, flags=0):
  63         can_pkt = self.sock.recv(72)
  64
  65         if len(can_pkt) == 16:
  66             can_id, length, data = struct.unpack(self.FORMAT, can_pkt)
  67         else:
  68             can_id, length, data = struct.unpack(self.FD_FORMAT, can_pkt)
  69
  70         can_id &= socket.CAN_EFF_MASK
  71         return (can_id, data[:length])
  72
  73 class VehicleSimulator(object):
  74     DEFAULT_IDLE_RPM = 600
  75
  76     def __init__(self):
  77         self.CRUISEMODE = False
  78         self.CRUISEACTIVE = False
  79         self.CRUISESPEED = 0
  80         self.CRUISERPM = 0
  81         self.freq = 10
  82         self.vehicle_speed = 0
  83         self.engine_speed = self.DEFAULT_IDLE_RPM
  84         self.thread = threading.Thread(target=self.run, daemon=True)
  85         self.lock = threading.Lock()
  86
  87     def reset(self):
  88         with self.lock:
  89             self.vehicle_speed = 0
  90             self.engine_speed = self.DEFAULT_IDLE_RPM
  91
  92     def start(self):
  93         self.thread.start()
  94
  95     def get_engine_speed(self):
  96         with self.lock:
  97             return int(self.engine_speed)
  98
  99     def get_vehicle_speed(self):
 100         with self.lock:
 101             return int(self.vehicle_speed)
 102
 103     def accelerate(self, target_speed, target_rpm, duration, bycruise = False):
 104         if target_speed <= self.vehicle_speed:
 105             return
 106         v = (target_speed - self.vehicle_speed) / (duration * self.freq)
 107         r = (target_rpm - self.engine_speed) / (duration * self.freq)
 108         while self.vehicle_speed < target_speed and (not self.CRUISEACTIVE or bycruise):
 109             with self.lock:
 110                 self.vehicle_speed += v;
 111                 self.engine_speed += r;
 112             time.sleep(1 / self.freq)
 113
 114     def brake(self, target_speed, target_rpm, duration, bycruise = False):
 115         if target_speed >= self.vehicle_speed:
 116             return
 117         v = (self.vehicle_speed - target_speed) / (duration * self.freq)
 118         r = (self.engine_speed - target_rpm) / (duration * self.freq)
 119         while self.vehicle_speed > target_speed and (not self.CRUISEACTIVE or bycruise):
 120             with self.lock:
 121                 self.vehicle_speed -= v;
 122                 self.engine_speed -= r;
 123             time.sleep(1 / self.freq)
 124
 125     def increase(self, bycruise = True):
 126         if self.CRUISEACTIVE:
 127             target_speed = self.vehicle_speed + 5
 128             target_rpm = self.engine_speed * 1.1
 129             self.accelerate(target_speed, target_rpm, 2, bycruise)
 130
 131     def decrease(self, bycruise = True):
 132         if self.CRUISEACTIVE:
 133             target_speed = self.vehicle_speed - 5
 134             target_rpm = self.engine_speed * 0.9
 135             self.brake(target_speed, target_rpm, 2, bycruise)
 136
 137     def resume(self, bycruise = True):
 138         target_speed = self.CRUISESPEED
 139         target_rpm = self.CRUISERPM
 140         current_speed = self.get_vehicle_speed()
 141         if target_speed > current_speed:
 142             self.accelerate(target_speed, target_rpm, 2, bycruise)
 143         else:
 144             self.brake(target_speed, target_rpm, 2, bycruise)
 145
 146     def run(self):
 147         while True:
 148             if not self.CRUISEACTIVE:
 149                 self.accelerate(80, 3000, 5)
 150                 self.accelerate(104, 4000, 3)
 151                 self.brake(80, 3000, 3)
 152                 self.accelerate(104, 4000, 6)
 153                 self.brake(40, 2000, 4)
 154                 self.accelerate(90, 3000, 5)
 155                 self.brake(1, 650, 5)
 156                 if not self.CRUISEACTIVE:
 157                     self.reset()
 158             time.sleep(5)
 159
 160 class DiagnosticMessageHandler(object):
 161     def __init__(self, can_sock, simulator, verbose=False):
 162         self.can_sock = can_sock
 163         self.simulator = simulator
 164         self.verbose = verbose
 165         self.thread = threading.Thread(target=self.run, daemon=True)
 166
 167     def start(self):
 168         self.thread.start()
 169
 170     def run(self):
 171         while True:
 172             can_id, data = self.can_sock.recv()
 173             #print('%03X#%s' % (can_id, ''.join(format(x, '02X') for x in data)))
 174             if can_id == 0x7df:
 175                 # OBD-II request
 176                 if data[1] == 0x01 and data[2] == 0x0C:
 177                     # Engine speed
 178                     speed = self.simulator.get_engine_speed()
 179                     #print('engine speed = %d' % speed)
 180                     if speed > 16383.75:
 181                         speed = 16383.75
 182                     reply = [ 0x04, 0x41, 0x0C ]
 183                     reply.append(4 * speed // 256)
 184                     reply.append(4 * speed % 256)
 185                     # pad remaining bytes to make 8
 186                     reply.append(0)
 187                     reply.append(0)
 188                     reply.append(0)
 189                     self.can_sock.send(0x7e8, bytes(reply), 0)
 190                 elif data[1] == 0x01 and data[2] == 0x0D:
 191                     # Vehicle speed
 192                     speed = int(self.simulator.get_vehicle_speed()) % 256
 193                     #print('vehicle speed = %d' % speed)
 194                     reply = [ 0x03, 0x41, 0x0D ]
 195                     reply.append(speed)
 196                     # pad remaining bytes to make 8
 197                     reply.append(0)
 198                     reply.append(0)
 199                     reply.append(0)
 200                     reply.append(0)
 201                     self.can_sock.send(0x7e8, bytes(reply), 0)
 202
 203 class SteeringWheelMessageHandler(object):
 204     def __init__(self, can_sock, simulator, verbose=False):
 205         self.can_sock = can_sock
 206         self.simulator = simulator
 207         self.verbose = verbose
 208         self.thread = threading.Thread(target=self.run, daemon=True)
 209         self.buttonpressed = False
 210         self.buttonenabled = False
 211         self.buttoncancel = False
 212         self.buttondec = False
 213         self.buttoninc = False
 214         self.cruisemode = False
 215         self.cruiseactive = False
 216
 217     def start(self):
 218         self.thread.start()
 219
 220     def run(self):
 221         while True:
 222             can_id, data = self.can_sock.recv()
 223             #print('%03X#%s' % (can_id, ''.join(format(x, '02X') for x in data)))
 224             if can_id == 0x21:
 225                 #print('%03X#%s' % (can_id, ''.join(format(x, '02X') for x in data)))
 226                 if data:
 227                     #if data[6]:
 228                         #print('data6: %02X' % (data[6]))
 229                     if data[6] == 0x80 and not self.buttonpressed:
 230                         # we do skip any further lin messages
 231                         # two buttons at the same time won't work
 232                         # (aka unlikely w/o twisting fingers)
 233                         self.buttonpressed = True
 234                         self.buttonenabled = True
 235                     if data[6] == 0x08 and not self.buttonpressed:
 236                         self.buttonpressed = True
 237                         self.buttoncancel = True
 238                     if data[6] == 0x10 and not self.buttonpressed:
 239                         self.buttonpressed = True
 240                         self.buttondec = True
 241                     if data[6] == 0x40 and not self.buttonpressed:
 242                         self.buttonpressed = True
 243                         self.buttoninc = True
 244                     if data[6] == 0x00 and self.buttonpressed:
 245                         #now handle it as the button was released
 246                         if self.buttonenabled:
 247                             self.buttonenabled = False
 248                             self.cruisemode = not self.cruisemode
 249                             #print("set cruisemode to %s" % self.cruisemode)
 250                             self.simulator.CRUISEMODE = self.cruisemode
 251                             # disable/reset all if going off
 252                             if not self.cruisemode:
 253                                 self.cruiseactive = False
 254                                 self.simulator.CRUISEACTIVE = self.cruiseactive
 255                                 self.simulator.CRUISESPEED = 0
 256                                 self.simulator.CRUISERPM = 0
 257                             #print("set cruiseactive to %s" % self.cruiseactive)
 258                         if self.buttoncancel:
 259                             self.buttoncancel = False
 260                             self.simulator.CRUISESPEED = self.simulator.get_vehicle_speed()
 261                             self.simulator.CRUISERPM = self.simulator.get_engine_speed()
 262                             #print("set cruisespeed to %d" % self.simulator.CRUISESPEED )
 263                             #print("set cruiserpm to %d" % self.simulator.CRUISERPM )
 264                             self.cruiseactive = False
 265                             #print("set cruiseactive to %s" % self.cruiseactive )
 266                             self.simulator.CRUISEACTIVE = self.cruiseactive
 267                         if self.buttondec:
 268                             self.buttondec = False
 269                             if self.cruiseactive:
 270                                 #print("decrease")
 271                                 self.simulator.decrease()
 272                             else:
 273                                 # set speed
 274                                 #print("set speed")
 275                                 self.simulator.CRUISESPEED = self.simulator.get_vehicle_speed()
 276                                 self.simulator.CRUISERPM = self.simulator.get_engine_speed()
 277                                 #print("set cruisespeed to %d" % self.simulator.CRUISESPEED )
 278                                 #print("set cruiserpm to %d" % self.simulator.CRUISERPM )
 279                                 self.cruiseactive = not self.cruiseactive
 280                                 #print("set cruiseactive to %s" % self.cruiseactive )
 281                                 self.simulator.CRUISEACTIVE = self.cruiseactive
 282                         if self.buttoninc:
 283                             self.buttoninc = False
 284                             if self.cruiseactive:
 285                                 #print("increase")
 286                                 self.simulator.increase()
 287                             else:
 288                                 if self.simulator.CRUISESPEED > 0:
 289                                     # resume
 290                                     self.cruiseactive = not self.cruiseactive
 291                                     self.simulator.CRUISEACTIVE = self.cruiseactive
 292                                     #print("set cruiseactive to %s" % self.cruiseactive )
 293                                     #print("resume")
 294                                     self.simulator.resume()
 295                         self.buttonpressed = False
 296
 297
 298 class StatusMessageSender(object):
 299     def __init__(self, can_sock, simulator, verbose=False):
 300         self.can_sock = can_sock
 301         self.simulator = simulator
 302         self.verbose = verbose
 303         self.thread = threading.Thread(target=self.run, daemon=True)
 304
 305     def start(self):
 306         self.thread.start()
 307
 308     def run(self):
 309         while True:
 310             # Engine speed
 311             speed = self.simulator.get_engine_speed()
 312             if self.verbose:
 313                 print('engine speed = %d' % speed)
 314             if speed > 16383.75:
 315                 speed = 16383.75
 316             # Message is 1 byte unknown, 1 byte fuel level, 2 bytes engine speed (4x), fuel low @ bit 55
 317             msg = [ 0, 0 ]
 318             speed *= 4
 319             msg.append(speed // 256)
 320             msg.append(speed % 256)
 321             # pad remaining bytes to make 8
 322             msg.append(0)
 323             msg.append(0)
 324             msg.append(0)
 325             msg.append(0)
 326             self.can_sock.send(0x3d9, bytes(msg), 0)
 327
 328             # Vehicle speed
 329             speed = int(self.simulator.get_vehicle_speed()) % 256
 330             if self.verbose:
 331                 print('vehicle speed = %d' % speed)
 332             # Message is 15 bits speed (64x), left aligned
 333             msg = [ ]
 334             # Note: extra 2x to yield required left-alignment
 335             speed *= 128
 336             msg.append(speed // 256)
 337             msg.append(speed % 256)
 338             # pad remaining bytes to make 8
 339             msg.append(0)
 340             msg.append(0)
 341             msg.append(0)
 342             msg.append(0)
 343             msg.append(0)
 344             msg.append(0)
 345             self.can_sock.send(0x3e9, bytes(msg), 0)
 346
 347             # Sleep 100 ms
 348             time.sleep(0.1)
 349
 350 def main():
 351     parser = argparse.ArgumentParser(description='Simple CAN vehicle simulator.')
 352     parser.add_argument('interface', type=str, help='interface name (e.g. vcan0)')
 353     parser.add_argument('-v', '--verbose', help='increase output verbosity', action='store_true')
 354     args = parser.parse_args()
 355
 356     try:
 357         can_sock = CANSocket(args.interface)
 358         diag_can_sock = CANSocket(args.interface)
 359         steeringwheel_can_sock = CANSocket(args.interface)
 360     except OSError as e:
 361         sys.stderr.write('Could not listen on interface {0}\n'.format(args.interface))
 362         sys.exit(e.errno)
 363
 364     print('Using {0}'.format(args.interface))
 365     sim = VehicleSimulator()
 366     status_sender = StatusMessageSender(can_sock, sim, args.verbose)
 367     diag_handler = DiagnosticMessageHandler(diag_can_sock, sim, args.verbose)
 368     steeringwheel_handler = SteeringWheelMessageHandler(steeringwheel_can_sock, sim, args.verbose)
 369     sim.start()
 370     status_sender.start()
 371     diag_handler.start()
 372     steeringwheel_handler.start()
 373     try:
 374         while True:
 375             time.sleep(60)
 376     except (KeyboardInterrupt, SystemExit):
 377         #sim.stop()
 378         sys.exit(0)
 379
 380 if __name__ == '__main__':
 381     main()