diff --git a/cereal/car.capnp b/cereal/car.capnp index 8fb312867687cc..fcd223acd43e8c 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -347,8 +347,9 @@ struct CarParams { longitudinalKpV @37 :List(Float32); longitudinalKiBP @38 :List(Float32); longitudinalKiV @39 :List(Float32); - steerLimitAlert @29 :Bool; + steerMPCReactTime @51 :Float32; + steerMPCDampTime @52 :Float32; vEgoStopping @30 :Float32; # Speed at which the car goes into stopping state directAccelControl @31 :Bool; # Does the car have direct accel control or just gas/brake diff --git a/cereal/log.capnp b/cereal/log.capnp index 3f79deefe54d33..644ffbdba419f4 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -397,6 +397,10 @@ struct Live100Data { jerkFactor @12 :Float32; angleSteers @13 :Float32; # Steering angle in degrees. angleSteersDes @29 :Float32; + dampAngleSteersDes @52 :Float32; + dampRateSteersDes @53 :Float32; + rateModeFF @54 :Float32; + angleModeFF @55 :Float32; curvature @37 :Float32; # path curvature from vehicle model hudLeadDEPRECATED @14 :Int32; cumLagMs @15 :Float32; @@ -613,6 +617,10 @@ struct PathPlan { angleSteers @8 :Float32; # deg rateSteers @13 :Float32; # deg/s + mpcAngles @14 :List(Float32); + mpcRates @15 :List(Float32); + mpcTimes @16 :List(Float32); + laneProb @17 :Float32; valid @9 :Bool; paramsValid @10 :Bool; modelValid @12 :Bool; @@ -1612,6 +1620,7 @@ struct LiveParametersData { angleOffsetAverage @3 :Float32; stiffnessFactor @4 :Float32; steerRatio @5 :Float32; + laneWidth @6 :Float32; } struct LiveMapData { diff --git a/dashboard.py b/dashboard.py new file mode 100644 index 00000000000000..f250878effa7c9 --- /dev/null +++ b/dashboard.py @@ -0,0 +1,534 @@ +#!/usr/bin/env python +import zmq +import time +import numpy as np +import math +from influxdb import InfluxDBClient, SeriesHelper +import selfdrive.messaging as messaging +from selfdrive.services import service_list +from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset +from selfdrive.controls.lib.pathplanner import PathPlanner +from selfdrive.controls.lib.vehicle_model import VehicleModel +from common.realtime import set_realtime_priority, Ratekeeper +from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv, calc_poly_curvature +import importlib +from collections import defaultdict, deque +from fastcluster import linkage_vector +from selfdrive.controls.lib.vehicle_model import VehicleModel +from cereal import car +from common.params import Params + +try: + from selfdrive.kegman_conf import kegman_conf +except: + pass + +def dashboard_thread(rate=300): + set_realtime_priority(5) + + USER = '' + PASSWORD = '' + DBNAME = 'carDB' + #influx = InfluxDBClient('192.168.1.61', 8086, USER, PASSWORD, DBNAME) + #influx = InfluxDBClient('192.168.43.221', 8086, USER, PASSWORD, DBNAME) + influx = InfluxDBClient('192.168.137.1', 8086, USER, PASSWORD, DBNAME) + + context = zmq.Context() + poller = zmq.Poller() + ipaddress = "127.0.0.1" + #carState = messaging.sub_sock(context, service_list['carState'].port, addr=ipaddress, conflate=True, poller=poller) + #can = messaging.sub_sock(context, service_list['can'].port, addr=ipaddress, poller=poller) + can = "disabled" + vEgo = 0.0 + #pathPlan = messaging.sub_sock(context, service_list['pathPlan'].port, addr=ipaddress, conflate=True, poller=poller) + pathPlan = None + live100 = messaging.sub_sock(context, service_list['live100'].port, addr=ipaddress, conflate=False, poller=poller) + #liveParameters = messaging.sub_sock(context, service_list['liveParameters'].port, addr=ipaddress, conflate=True, poller=poller) + liveParameters = None + #live20 = messaging.sub_sock(context, service_list['live20'].port, addr=ipaddress, conflate=True, poller=poller) + #model = messaging.sub_sock(context, service_list['model'].port, addr=ipaddress, conflate=True, poller=poller) + model = None + #frame = messaging.sub_sock(context, service_list['frame'].port, addr=ipaddress, conflate=True, poller=poller) + #sensorEvents = messaging.sub_sock(context, service_list['sensorEvents'].port, addr=ipaddress, conflate=True, poller=poller) + #carControl = messaging.sub_sock(context, service_list['carControl'].port, addr=ipaddress, conflate=True, poller=poller) + #health = messaging.sub_sock(context, service_list['health'].port, addr=ipaddress, conflate=True, poller=poller) + #sendcan = messaging.sub_sock(context, service_list['sendcan'].port, addr=ipaddress, conflate=True, poller=poller) + #androidLog = messaging.sub_sock(context, service_list['androidLog'].port, addr=ipaddress, conflate=True, poller=poller) + #carState = "disabled" + androidLog = "disabled" + sendcan = "disabled" + health = "disabled" + carControl = "disabled" + sensorEvents = "disabled" + frame = "disabled" + #model = "disabled" + live20 = "disabled" + + _model = None + _live100 = None + _liveParameters = None + _pathPlan = None + _live20 = None + _carState = None + _can = None + _frame = None + _sensorEvents = None + _carControl = None + _health = None + _sendcan = None + _androidLog = None + + frame_count = 0 + can_count = 0 + sample_str = "" + canDataString = "" + influxLineString = "" + captureResonantParams = True + skippedLiveParameters = 0 + l_poly = [0., 0., 0., 0.] + r_poly = [0., 0., 0., 0.] + prev_l_curv = None + prev_r_curv = None + prev_p_curv = None + prev_l_sum = 0 + prev_r_sum = 0 + prev_p_sum = 0 + + current_rate = rate + rk = Ratekeeper(current_rate, print_delay_threshold=np.inf) + + kegman_counter = 0 + monoTimeOffset = 0 + receiveTime = 0 + + CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) + VM = VehicleModel(CP) + _path_pinv = compute_path_pinv() + + while 1: + #try: + #receiveTime = int(time.time() * 1000000000) + for socket, event in poller.poll(0): + if socket is live100: + #l100 = messaging.recv_one(socket) + _live100 = messaging.drain_sock(socket) + for l100 in _live100: + vEgo = l100.live100.vEgo + if vEgo > 0: # and l100.live100.active: + #if sample_str != "": + # sample_str += "," + receiveTime = int(monoTimeOffset + l100.logMonoTime) + #print(receiveTime, monoTimeOffset, l100.logMonoTime) + if (abs(receiveTime - int(time.time() * 1000000000)) > 10000000000): + angle_error_noise = 0.0 + last_desired = 0.0 + last_actual = 0.0 + actual_angle_change_noise = 0.0 + desired_angle_change_noise = 0.0 + angle_error_noise = 0.0 + monoTimeOffset = (time.time() * 1000000000) - l100.logMonoTime + receiveTime = int(monoTimeOffset + l100.logMonoTime) + print(int(time.time() * 1000000000), receiveTime, monoTimeOffset, l100.logMonoTime) + #receiveTime = 1 / 0 + abs_error = abs(l100.live100.angleSteers - l100.live100.angleSteersDes) + angle_error_noise = ((99. * angle_error_noise) + (math.pow(abs_error, 2.))) / 100. + abs_desired_change = abs(l100.live100.angleSteersDes - last_desired) + desired_angle_change_noise = ((99. * desired_angle_change_noise) + (math.pow(abs_desired_change, 2.))) / 100. + abs_angle_change = abs(l100.live100.angleSteersDes - last_actual) + actual_angle_change_noise = ((99. * actual_angle_change_noise) + (math.pow(abs_angle_change, 2.))) / 100. + last_desired = l100.live100.angleSteersDes + last_actual = l100.live100.angleSteers + v_curv = l100.live100.curvature + + influxLineString += ("opData,sources=capnp ff_rate=%1.3f,ff_angle=%1.3f,ang_err_noise=%1.1f,des_noise=%1.1f,ang_noise=%1.1f,angle_steers_des=%1.2f,angle_steers=%1.2f,dampened_angle_steers_des=%1.2f,v_ego=%1.2f,steer_override=%1.2f,v_ego=%1.4f,p=%1.2f,i=%1.4f,f=%1.4f,cumLagMs=%1.2f,vCruise=%1.2f %s\n" % + (l100.live100.rateModeFF, l100.live100.angleModeFF, angle_error_noise, desired_angle_change_noise, actual_angle_change_noise, l100.live100.angleSteersDes, l100.live100.angleSteers, l100.live100.dampAngleSteersDes, l100.live100.vEgo, l100.live100.steerOverride, l100.live100.vPid, + l100.live100.upSteer, l100.live100.uiSteer, l100.live100.ufSteer, l100.live100.cumLagMs, l100.live100.vCruise, receiveTime)) + frame_count += 1 + + '''print(_live100) + live100 = ( + vEgo = 0, + aEgoDEPRECATED = 0, + vPid = 0.3, + vTargetLead = 0, + upAccelCmd = 0, + uiAccelCmd = 0, + yActualDEPRECATED = 0, + yDesDEPRECATED = 0, + upSteer = 0, + uiSteer = 0, + aTargetMinDEPRECATED = 0, + aTargetMaxDEPRECATED = 0, + jerkFactor = 0, + angleSteers = -1, + hudLeadDEPRECATED = 0, + cumLagMs = -0.57132614, + canMonoTimeDEPRECATED = 0, + l20MonoTimeDEPRECATED = 0, + mdMonoTimeDEPRECATED = 0, + enabled = false, + steerOverride = false, + canMonoTimes = [], + vCruise = 255, + rearViewCam = false, + alertText1 = "", + alertText2 = "", + awarenessStatus = 0, + angleOffset = -0.74244022, + planMonoTime = 10166703166901, + angleSteersDes = -1, + longControlState = off, + state = disabled, + vEgoRaw = 0, + ufAccelCmd = 0, + ufSteer = 0, + aTarget = 0, + active = false, + curvature = -0.00038641863, + alertStatus = normal, + alertSize = none, + gpsPlannerActive = false, + engageable = false, + alertBlinkingRate = 0, + driverMonitoringOn = false, + alertType = "", + alertSound = "" ) )''' + elif socket is model: + _model = messaging.recv_one(socket) + md = _model.model + if vEgo > 0: # and l100.live100.active: + influxLineString += ("opLines,sources=capnp ") + influxLineString += ("l0=%1.3f,p0=%1.3f,r0=%1.3f" % (md.leftLane.points[0], md.path.points[0], md.rightLane.points[0])) + lp = md.leftLane.points + rp = md.rightLane.points + pp = md.path.points + + p_sum = np.sum(md.path.points) + l_sum = np.sum(md.leftLane.points) + r_sum = np.sum(md.rightLane.points) + + l_change = l_sum - prev_l_sum + r_change = r_sum - prev_r_sum + p_change = p_sum - prev_p_sum + prev_l_sum = l_sum + prev_r_sum = r_sum + prev_p_sum = p_sum + + + p_poly = model_polyfit(md.path.points, _path_pinv) + l_poly = model_polyfit(md.leftLane.points, _path_pinv) + r_poly = model_polyfit(md.rightLane.points, _path_pinv) + p_curv = calc_poly_curvature(p_poly) + l_curv = calc_poly_curvature(l_poly) + r_curv = calc_poly_curvature(r_poly) + + '''far_pinv = [_path_pinv[0][25:50],_path_pinv[1][25:50],_path_pinv[2][25:50],_path_pinv[3][25:50]] + near_pinv = [_path_pinv[0][0:30],_path_pinv[1][0:30],_path_pinv[2][0:30],_path_pinv[3][0:30]] + + p_poly_far = model_polyfit(map(float, md.path.points)[25:50], far_pinv) # predicted path + l_poly_far = model_polyfit(map(float, md.leftLane.points)[25:50], far_pinv) # left line + r_poly_far = model_polyfit(map(float, md.rightLane.points)[25:50], far_pinv) # right line + + p_poly_near = model_polyfit(map(float, md.path.points)[0:30], near_pinv) # predicted path + l_poly_near = model_polyfit(map(float, md.leftLane.points)[0:30], near_pinv) # left line + r_poly_near = model_polyfit(map(float, md.rightLane.points)[0:30], near_pinv) # right line + + p_curv1 = calc_poly_curvature(p_poly_far) + l_curv1 = calc_poly_curvature(l_poly_far) + r_curv1 = calc_poly_curvature(r_poly_far) + p_curv2 = calc_poly_curvature(p_poly_near) + l_curv2 = calc_poly_curvature(l_poly_near) + r_curv2 = calc_poly_curvature(r_poly_near) + ''' + #left_curv = calc_poly_curvature([lp[0], lp[9], lp[19], lp[29], lp[39],lp[49]]) + #path_curv = calc_poly_curvature([pp[0], pp[9], pp[19], pp[29], pp[39],pp[49]]) + #path_curv = calc_poly_curvature(md.path.points) + #right_curv = calc_poly_curvature([rp[0], rp[9], rp[19], rp[29], rp[39],rp[49]]) #calc_poly_curvature(md.rightLane.points) + #print(left_curv, path_curv, right_curv) + for i in range(5,50,5): + influxLineString += (",l%d=%1.3f,p%d=%1.3f,r%d=%1.3f" % (i, md.leftLane.points[i], i, md.path.points[i], i, md.rightLane.points[i])) + #influxLineString += (",left_curv=%1.1f %s\n" % (left_curv, receiveTime)) + #influxLineString += (",vEgo=%1.1f,vCurv=%1.5f,lstd=%1.2f,rstd=%1.2f,pstd=%1.2f,lsum=%d,rsum=%d,psum=%d,lchange=%d,rchange=%d,pchange=%d,lProb=%1.2f,pProb=%1.2f,rProb=%1.2f,p_curv1=%1.5f,l_curv1=%1.5f,r_curv1=%1.5f,p_curv2=%1.5f,l_curv2=%1.5f,r_curv2=%1.5f,v_curv=%1.5f,p_curv=%1.5f,l_curv=%1.5f,r_curv=%1.5f %s\n" % \ + # (vEgo, v_curv, md.leftLane.std,md.rightLane.std,md.path.std,l_sum, r_sum, p_sum, l_change,r_change,p_change,md.leftLane.prob, md.path.prob, md.rightLane.prob,p_curv1,l_curv1,r_curv1,p_curv2,l_curv2,r_curv2, v_curv, p_curv,l_curv, r_curv, receiveTime)) + influxLineString += (",vEgo=%1.1f,lstd=%1.1f,rstd=%1.1f,lsum=%d,rsum=%d,psum=%d,lchange=%d,rchange=%d,pchange=%d,lProb=%1.2f,rProb=%1.2f,v_curv=%1.5f,p_curv=%1.5f,l_curv=%1.5f,r_curv=%1.5f %s\n" % \ + (vEgo, md.leftLane.std, md.rightLane.std, l_sum, r_sum, p_sum, l_change,r_change,p_change,md.leftLane.prob, md.rightLane.prob,v_curv, p_curv,l_curv, r_curv, receiveTime)) + + frame_count += 1 + + '''model = ( + frameId = 19786, + path = ( + points = [-0.002040863, 0.0048789978, 0.0024032593, -0.029251099, -0.050567627, -0.071716309, -0.10424805, -0.14196777, -0.18005371, -0.20825195, -0.2277832, -0.26391602, -0.31420898, -0.38085938, -0.43212891, -0.47900391, -0.51318359, -0.56494141, -0.62646484, -0.68212891, -0.73632812, -0.74951172, -0.82519531, -0.89648438, -0.97265625, -1.0615234, -1.1464844, -1.2412109, -1.3369141, -1.4462891, -1.5488281, -1.6445312, -1.7460938, -1.8544922, -1.9658203, -2.0820312, -2.2089844, -2.3320312, -2.484375, -2.6152344, -2.7265625, -2.8554688, -2.984375, -3.1425781, -3.2636719, -3.4160156, -3.5566406, -3.6835938, -3.8222656, -3.9746094], + prob = 1, + std = -0.66490114 ), + leftLane = ( + points = [1.7112548, 1.7149169, 1.720166, 1.7105224, 1.704602, 1.7070434, 1.7025878, 1.6870239, 1.6792724, 1.6579101, 1.6454589, 1.633374, 1.6216552, 1.6204345, 1.5994384, 1.5890625, 1.5706298, 1.5534179, 1.539746, 1.5231445, 1.5058105, 1.4767578, 1.4601562, 1.4442871, 1.4245117, 1.407666, 1.3986328, 1.3683593, 1.340039, 1.3173339, 1.2873046, 1.2677734, 1.234082, 1.199414, 1.1696289, 1.1339843, 1.1095703, 1.0788085, 1.0495117, 1.0299804, 0.98798829, 0.95527345, 0.925, 0.89277345, 0.86982423, 0.81464845, 0.77656251, 0.75117189, 0.71308595, 0.66328126], + prob = 0.11716748, + std = -3.1076281 ), + rightLane = ( + points = [-2.0839355, -2.0990722, -2.1112792, -2.1142089, -2.1159179, -2.1269042, -2.1374023, -2.1483886, -2.1569335, -2.1713378, -2.1752441, -2.1828125, -2.1942871, -2.2208984, -2.2377441, -2.2616699, -2.3014648, -2.3073242, -2.3302734, -2.3463867, -2.3625, -2.37666, -2.3898437, -2.4123046, -2.4396484, -2.4586914, -2.4953125, -2.5304687, -2.5529296, -2.5724609, -2.6032226, -2.6344726, -2.6637695, -2.6911132, -2.727246, -2.758496, -2.7921875, -2.8205078, -2.8458984, -2.8791015, -2.9230468, -2.9416015, -2.9894531, -3.0148437, -3.0392578, -3.0695312, -3.1144531, -3.1388671, -3.1652343, -3.1916015], + prob = 0.095686942, + std = -2.2504346 ), + lead = (dist = 13.424072, prob = 0.37279058, std = 16.329063), + settings = ( + bigBoxX = 0, + bigBoxY = 0, + bigBoxWidth = 0, + bigBoxHeight = 0, + inputTransform = [1.25, 0, 374, 0, 1.25, 337.75, 0, 0, 1] ) ) )''' + + elif socket is liveParameters: + _liveParameters = messaging.recv_one(socket) + lp = _liveParameters.liveParameters + skippedLiveParameters += 1 + if vEgo >= 0 and skippedLiveParameters >= 10: # and _live100.live100.active: + skippedLiveParameters= 0 + if sample_str != "": + sample_str += "," + sample_str = ("angleOffset=%1.2f,angleOffsetAverage=%1.3f,stiffnessFactor=%1.3f,steerRatio=%1.3f,laneWidtb=%1.1f" % + (lp.angleOffset, lp.angleOffsetAverage, lp.stiffnessFactor, lp.steerRatio, lp.laneWidth)) + influxLineString += ("opData,sources=capnp " + sample_str + " %s\n" % receiveTime) + sample_str = "" + frame_count += 1 + + elif socket is pathPlan: + _pathPlan = messaging.recv_one(socket) + if vEgo > 0: # and _live100.live100.active: + if sample_str != "": + sample_str += "," + a = _pathPlan.pathPlan.mpcAngles + #r = _pathPlan.pathPlan.mpcRates + p = _pathPlan.pathPlan + + #sample_str = ("lane_width=%1.2f,lpoly2=%1.3f,rpoly2=%1.3f,cpoly2=%1.3f,dpoly2=%1.3f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f,mpc4=%1.2f,mpc5=%1.2f,mpc6=%1.2f,mpc7=%1.2f,mpc8=%1.2f,mpc9=%1.2f,mpc10=%1.2f,mpc11=%1.2f,mpc12=%1.2f,mpc13=%1.2f,mpc14=%1.2f,mpc15=%1.2f,mpc16=%1.2f,mpc17=%1.2f,mpc18=%1.2f" % + # (_pathPlan.pathPlan.laneWidth, _pathPlan.pathPlan.lPoly[2], _pathPlan.pathPlan.rPoly[2], _pathPlan.pathPlan.cPoly[2], _pathPlan.pathPlan.dPoly[2],_pathPlan.pathPlan.lPoly[3], _pathPlan.pathPlan.rPoly[3], _pathPlan.pathPlan.cPoly[3], _pathPlan.pathPlan.dPoly[3], + # _pathPlan.pathPlan.cProb, _pathPlan.pathPlan.lProb, _pathPlan.pathPlan.rProb, a[1], a[2], a[3], + # a[4], a[5], a[6], a[7], a[8], a[9], a[10], a[11], a[12], a[13], a[14], a[15], a[16], a[17], a[18])) + sample_str = ("lane_width=%1.2f,lpoly0=%1.6f,rpoly0=%1.6f,cpoly0=%1.6f,dpoly0=%1.6f,lpoly1=%1.5f,rpoly1=%1.5f,cpoly1=%1.5f,dpoly1=%1.5f,lpoly2=%1.4f,rpoly2=%1.4f,cpoly2=%1.4f,dpoly2=%1.4f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc0=%1.2f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f,mpc4=%1.2f,mpc5=%1.2f,mpc6=%1.2f" % + (p.laneWidth, p.lPoly[0], p.rPoly[0], p.cPoly[0], p.dPoly[0], p.lPoly[1], p.rPoly[1], p.cPoly[1], p.dPoly[1], p.lPoly[2], p.rPoly[2], p.cPoly[2], p.dPoly[2],p.lPoly[3], p.rPoly[3], p.cPoly[3], p.dPoly[3], + p.cProb, p.lProb, p.rProb, a[0], a[1], a[2], a[3], a[4], a[5], a[6])) + influxLineString += ("opData,sources=capnp " + sample_str + " %s\n" % receiveTime) + sample_str = "" + frame_count += 1 + + + #elif socket is live20: + # _live20 = messaging.recv_one(socket) + '''live20 = ( + angleOffsetDEPRECATED = 0, + calStatusDEPRECATED = 0, + leadOne = ( + dRel = 0, + yRel = 0, + vRel = 0, + aRel = 0, + vLead = 0, + aLeadDEPRECATED = 0, + dPath = 0, + vLat = 0, + vLeadK = 0, + aLeadK = 0, + fcw = false, + status = false, + aLeadTau = 0 ), + cumLagMs = 7678.7031, + mdMonoTime = 10166684697265, + ftMonoTimeDEPRECATED = 0, + calCycleDEPRECATED = 0, + calPercDEPRECATED = 0, + canMonoTimes = [], + l100MonoTime = 10166705048151, + radarErrors = [] ) )''' + elif socket is carState: + _carState = messaging.recv_one(socket) + if vEgo > 0: + if sample_str != "": + sample_str += "," + sample_str += ("lead_distance=%d,driver_torque=%1.2f,angle_rate=%1.2f,yawRate=%1.2f,angle_steers=%1.2f" % (_carState.carState.leadDistance, _carState.carState.steeringTorque, _carState.carState.steeringRate, _carState.carState.yawRate, _carState.carState.steeringAngle)) + + '''carState = ( + vEgo = 0, + wheelSpeeds = (fl = 0, fr = 0, rl = 0, rr = 0), + gas = 0, + gasPressed = false, + brake = 0, + brakePressed = false, + steeringAngle = -1, + steeringTorque = 84, + steeringPressed = false, + cruiseState = ( + enabled = false, + speed = 0, + available = true, + speedOffset = -0.3, + standstill = false ), + buttonEvents = [], + canMonoTimes = [], + events = [ + ( name = wrongGear, + enable = false, + noEntry = true, + warning = false, + userDisable = false, + softDisable = true, + immediateDisable = false, + preEnable = false, + permanent = false ), + gearShifter = park, + steeringRate = 0, + aEgo = 0, + vEgoRaw = 0, + standstill = true, + brakeLights = false, + leftBlinker = false, + rightBlinker = false, + yawRate = -0, + genericToggle = false, + doorOpen = false, + seatbeltUnlatched = true ) )''' + elif socket is can: + recordBus = [2,] + recordPid = [228,] + receiveTime = int(time.time() * 1000000000) + words = [0,0,0,0] + for _can in messaging.drain_sock(socket): + for msg in _can.can: + can_count += 1 + if msg.src in recordBus and (len(recordPid) == 0 or msg.address in recordPid): + All64 = int(msg.dat.encode("hex"), 16) + Second32 = All64 & 0xFFFFFFFF + words[3] = Second32 & 0xFFFF + words[2] = Second32 >> 16 + First32 = All64 >> 32 + words[1] = First32 & 0xFFFF + words[0] = First32 >> 16 + canDataString += ('rawCANData,pid=%s,bus=%s First32=%di,Second32=%di,word1=%di,word2=%di,word3=%di,word4=%di %d\n' % + (msg.address, msg.src, First32, Second32, words[0], words[1], words[2], words[3], receiveTime)) + '''print(_can) + can = [ + ( address = 513, + busTime = 42188, + dat = "", + src = 1 ),''' + #elif socket is frame: + # _frame = messaging.recv_one(socket) + '''frame = ( + frameId = 14948, + encodeId = 14947, + timestampEof = 10728391665000, + frameLength = 5419, + integLines = 601, + globalGain = 509, + frameType = unknown, + timestampSof = 0, + transform = [1, 0, 0, 0, 1, 0, 0, 0, 1], + lensPos = 281, + lensSag = -0.59991455, + lensErr = 54, + lensTruePos = 273.13062 ) )''' + #elif socket is sensorEvents: + # _sensorEvents = messaging.recv_one(socket) + '''sensorEvents = [ + ( version = 104, + sensor = 2, + type = 2, + timestamp = 10551787945172, + magnetic = ( + v = [-40.446472, 92.475891, 17.285156], + status = 2 ), + source = android, + uncalibratedDEPRECATED = false ), + ( version = 104, + sensor = 5, + type = 16, + timestamp = 10551844372174, + source = android, + uncalibratedDEPRECATED = false, + gyroUncalibrated = ( + v = [-0.00062561035, -0.0029144287, -0.039916992, 1.5258789e-05, -0.0028686523, -0.039474487], + status = 0 ) ) ] )''' + #elif socket is carControl: + # _carControl = messaging.recv_one(socket) + '''carControl = ( + enabled = false, + gasDEPRECATED = 0, + brakeDEPRECATED = 0, + steeringTorqueDEPRECATED = 0, + cruiseControl = ( + cancel = false, + override = true, + speedOverride = 0, + accelOverride = 0.714 ), + hudControl = ( + speedVisible = false, + setSpeed = 70.833336, + lanesVisible = false, + leadVisible = false, + visualAlert = none, + audibleAlert = none ), + actuators = (gas = 0, brake = -0, steer = 0, steerAngle = -2.7103169), + active = false ) )''' + #elif socket is _health: + # _health = messaging.recv_one(socket) + # print(_health) + #elif socket is _sendcan: + # _sendcan = messaging.recv_one(socket) + # print(_sendcan) + #elif socket is _androidLog: + # _androidLog = messaging.recv_one(socket) + # print(_androidLog) + + #print(influxLineString) + if sample_str != "": + ''' + kegman_counter += 1 + if kegman_counter == 300: + kegman_counter = 0 + try: + kegman = kegman_conf() + mpc_project = kegman.conf['reactMPC'] + mpc_smooth = kegman.conf['dampMPC'] + steer_project = kegman.conf['reactSteer'] + steer_smooth = kegman.conf['dampSteer'] + steerKpV = kegman.conf['Kp'] + steerKiV = kegman.conf['Ki'] + sample_str += (",mpc_project=%s,mpc_smooth=%s,steer_project=%s,steer_smooth=%s,KpV=%s,KiV=%s" % (mpc_project,mpc_smooth,steer_project,steer_smooth, steerKpV, steerKiV)) + except: + pass + ''' + influxLineString += ("opData,sources=capnp " + sample_str + " %s\n" % receiveTime) + frame_count += 1 + sample_str = "" + if canDataString != "": + influxLineString += canDataString + frame_count += 1 + canDataString = "" + + if frame_count >= 10: + #print(influxLineString) + headers = { 'Content-type': 'application/octet-stream', 'Accept': 'text/plain' } + try: + influx.request("write",'POST', {'db':DBNAME}, influxLineString.encode('utf-8'), 204, headers) + if current_rate != rate: + current_rate = rate + rk = Ratekeeper(current_rate, print_delay_threshold=np.inf) + print ('%d %d' % (frame_count, len(influxLineString))) + except: + if current_rate != 1: + current_rate = 1 + rk = Ratekeeper(current_rate, print_delay_threshold=np.inf) + continue + frame_count = 0 + can_count = 0 + influxLineString = "" + + rk.keep_time() + #except expression as identifier: + # print(identifier) + # pass + +def main(rate=300): + dashboard_thread(rate) + +if __name__ == "__main__": + main() diff --git a/dashfile.py b/dashfile.py new file mode 100644 index 00000000000000..f320f40bf48c71 --- /dev/null +++ b/dashfile.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python +import csv +import zmq +import time +import numpy as np +import selfdrive.messaging as messaging +from selfdrive.services import service_list +from common.realtime import set_realtime_priority, Ratekeeper +import os, os.path + +# Polling rate should be twice the data rate to prevent aliasing +def main(rate=100): + set_realtime_priority(5) + context = zmq.Context() + poller = zmq.Poller() + + live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=False, poller=poller) + carState = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller) + can = None #messaging.sub_sock(context, service_list['can'].port, conflate=True, poller=poller) + + vEgo = 0.0 + _live100 = None + _can = None + + frame_count = 0 + skipped_count = 0 + + rk = Ratekeeper(rate, print_delay_threshold=np.inf) + + # simple version for working with CWD + #print len([name for name in os.listdir('.') if os.path.isfile(name)]) + + # path joining version for other paths + DIR = '/sdcard/tuning' + filenumber = len([name for name in os.listdir(DIR) if os.path.isfile(os.path.join(DIR, name))]) + + kegman_counter = 0 + monoTimeOffset = 0 + receiveTime = 0 + angle_rate = 0.0 + + print("start") + with open(DIR + '/dashboard_file_%d.csv' % filenumber, mode='w') as dash_file: + print("opened") + dash_writer = csv.writer(dash_file, delimiter=',', quotechar='', quoting=csv.QUOTE_NONE) + print("initialized") + dash_writer.writerow(['ff_rate','ff_angle', 'angle_steers_des','angle_steers','angle_rate','dampened_angle_steers_des','dampened_angle_rate_des','dampened_angle_steers','v_ego','steer_override','p','i','f','time']) + print("first row") + + while 1: + for socket, event in poller.poll(0): + if socket is can: + _can = messaging.recv_one(socket) + print(_can) + + if socket is carState: + _carState = messaging.drain_sock(socket) + for cs in _carState: + angle_rate = cs.carState.steeringRate + + if socket is live100: + _live100 = messaging.drain_sock(socket) + for l100 in _live100: + vEgo = l100.live100.vEgo + if vEgo > 0: # and l100.live100.active: + receiveTime = int(monoTimeOffset + l100.logMonoTime) + if (abs(receiveTime - int(time.time() * 1000000000)) > 10000000000): + monoTimeOffset = (time.time() * 1000000000) - l100.logMonoTime + receiveTime = int(monoTimeOffset + l100.logMonoTime) + + frame_count += 1 + dash_writer.writerow([str(round(l100.live100.rateModeFF, 2)), + str(round(l100.live100.angleModeFF, 2)), + str(round(l100.live100.angleSteersDes, 2)), + str(round(l100.live100.angleSteers, 2)), + str(round(angle_rate, 2)), + str(round(l100.live100.dampAngleSteersDes, 2)), + str(round(l100.live100.dampAngleRateDes, 2)), + str(round(l100.live100.dampAngleSteers, 2)), + str(round(l100.live100.vEgo, 1)), + 1 if l100.live100.steerOverride else 0, + str(round(l100.live100.upSteer, 4)), + str(round(l100.live100.uiSteer, 4)), + str(round(l100.live100.ufSteer, 4)), + str(receiveTime)]) + else: + skipped_count += 1 + else: + skipped_count += 1 + if frame_count % 200 == 0: + print("captured = %d" % frame_count) + frame_count += 1 + if skipped_count % 200 == 0: + print("skipped = %d" % skipped_count) + skipped_count += 1 + + rk.keep_time() + +if __name__ == "__main__": + main() diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 41f8a607cf9ca5..b4f148956a1c9f 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -6,12 +6,14 @@ fi function launch { # apply update + ''' if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then git reset --hard @{u} && git clean -xdf && exec "${BASH_SOURCE[0]}" fi - + ''' + # no cpu rationing for now echo 0-3 > /dev/cpuset/background/cpus echo 0-3 > /dev/cpuset/system-background/cpus diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index dd89d4d6d48bda..12712d09013359 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -65,6 +65,9 @@ pthread_t safety_setter_thread_handle = -1; pthread_t pigeon_thread_handle = -1; bool pigeon_needs_init; +int big_recv; +uint32_t big_data[RECV_SIZE*2]; + void pigeon_init(); void *pigeon_thread(void *crap); @@ -216,11 +219,14 @@ void handle_usb_issue(int err, const char func[]) { // TODO: check other errors, is simply retrying okay? } -void can_recv(void *s) { +bool can_recv(void *s, bool force_send) { int err; uint32_t data[RECV_SIZE/4]; - int recv; - uint32_t f1, f2; + int recv, big_index; + uint32_t f1, f2, address; + bool frame_sent; + uint64_t cur_time; + frame_sent = false; // do recv pthread_mutex_lock(&usb_lock); @@ -236,38 +242,52 @@ void can_recv(void *s) { pthread_mutex_unlock(&usb_lock); - // return if length is 0 - if (recv <= 0) { - return; + // return if both buffers are empty + if ((big_recv <= 0) && (recv <= 0)) { + return true; } - // create message - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); + big_index = big_recv/0x10; + for (int i = 0; i<(recv/0x10); i++) { + big_data[(big_index + i)*4] = data[i*4]; + big_data[(big_index + i)*4+1] = data[i*4+1]; + big_data[(big_index + i)*4+2] = data[i*4+2]; + big_data[(big_index + i)*4+3] = data[i*4+3]; + big_recv += 0x10; + } + if (force_send) { + frame_sent = true; - auto canData = event.initCan(recv/0x10); + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); - // populate message - for (int i = 0; i<(recv/0x10); i++) { - if (data[i*4] & 4) { - // extended - canData[i].setAddress(data[i*4] >> 3); - //printf("got extended: %x\n", data[i*4] >> 3); - } else { - // normal - canData[i].setAddress(data[i*4] >> 21); + auto can_data = event.initCan(big_recv/0x10); + + // populate message + for (int i = 0; i<(big_recv/0x10); i++) { + if (big_data[i*4] & 4) { + // extended + can_data[i].setAddress(big_data[i*4] >> 3); + //printf("got extended: %x\n", big_data[i*4] >> 3); + } else { + // normal + can_data[i].setAddress(big_data[i*4] >> 21); + } + can_data[i].setBusTime(big_data[i*4+1] >> 16); + int len = big_data[i*4+1]&0xF; + can_data[i].setDat(kj::arrayPtr((uint8_t*)&big_data[i*4+2], len)); + can_data[i].setSrc((big_data[i*4+1] >> 4) & 0xff); } - canData[i].setBusTime(data[i*4+1] >> 16); - int len = data[i*4+1]&0xF; - canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len)); - canData[i].setSrc((data[i*4+1] >> 4) & 0xff); + + // send to can + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + zmq_send(s, bytes.begin(), bytes.size(), 0); + big_recv = 0; } - // send to can - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - zmq_send(s, bytes.begin(), bytes.size(), 0); + return frame_sent; } void can_health(void *s) { @@ -456,11 +476,53 @@ void *can_recv_thread(void *crap) { void *publisher = zmq_socket(context, ZMQ_PUB); zmq_bind(publisher, "tcp://*:8006"); - // run at ~200hz + bool frame_sent, skip_once, force_send; + uint64_t wake_time, cur_time, last_long_sleep; + int recv_state = 0; + force_send = true; + last_long_sleep = 1e-3 * nanos_since_boot(); + wake_time = last_long_sleep; + while (!do_exit) { - can_recv(publisher); - // 5ms - usleep(5*1000); + + frame_sent = can_recv(publisher, force_send); + + // drain the Panda twice at 4.5ms intervals, then once at 1.0ms interval (twice max if sync_id is set) + if (recv_state++ < 2) { + last_long_sleep = 1e-3 * nanos_since_boot(); + wake_time += 4500; + force_send = false; + if (last_long_sleep < wake_time) { + usleep(wake_time - last_long_sleep); + } + else { + if ((last_long_sleep - wake_time) > 5e5) { + // probably a new drive + wake_time = last_long_sleep; + } + else { + if (recv_state < 2) { + wake_time += 4500; + recv_state++; + if (last_long_sleep < wake_time) { + usleep(wake_time - last_long_sleep); + } + else { + printf(" lagging!\n"); + } + } + } + } + } + else { + force_send = true; + recv_state = 0; + wake_time += 1000; + cur_time = 1e-3 * nanos_since_boot(); + if (wake_time > cur_time) { + usleep(wake_time - cur_time); + } + } } return NULL; } @@ -644,7 +706,7 @@ int main() { LOGW("starting boardd"); // set process priority - err = set_realtime_priority(4); + err = set_realtime_priority(3); LOG("setpriority returns %d", err); // check the environment diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index b6845b47db2ad9..e7b67602db5197 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -334,15 +334,19 @@ class CANParser { // multiple recv is fine bool first = wait; - while (1) { + while (first || drain) { if (first) { err = zmq_msg_recv(&msg, subscriber, 0); first = false; } else { + // Drain the queue at startup + usleep(500); err = zmq_msg_recv(&msg, subscriber, ZMQ_DONTWAIT); } - if (err < 0) break; - + if (err < 0) { + drain = false; + break; + } // format for board, make copy due to alignment issues, will be freed on out of scope auto amsg = kj::heapArray((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1); memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg)); @@ -354,10 +358,9 @@ class CANParser { auto cans = event.getCan(); UpdateCans(sec, cans); + UpdateValid(sec); } - UpdateValid(sec); - zmq_msg_close(&msg); } @@ -386,6 +389,7 @@ class CANParser { private: const int bus; + bool drain = true; // zmq vars void *context = NULL; void *subscriber = NULL; diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index ac797616bc32ab..1a3cad4327f981 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -78,6 +78,8 @@ def get_params(candidate, fingerprint): ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.7 + ret.steerMPCReactTime = 0.025 # increase total MPC projected time by 25 ms + ret.steerMPCDampTime = 0.25 # dampen desired angle over 250ms (5 mpc cycles) if candidate == CAR.JEEP_CHEROKEE: ret.wheelbase = 2.91 # in meters @@ -138,7 +140,7 @@ def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] - self.cp.update(int(sec_since_boot() * 1e9), False) + self.cp.update(int(sec_since_boot() * 1e9), True) self.CS.update(self.cp) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 0e29ea62620814..c9057e8670c762 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -77,6 +77,8 @@ def get_params(candidate, fingerprint): ret.steerKf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerRateCost = 1.0 + ret.steerMPCReactTime = 0.025 # increase total MPC projected time by 25 ms + ret.steerMPCDampTime = 0.25 # dampen desired angle over 250ms (5 mpc cycles) f = 1.2 tireStiffnessFront_civic *= f @@ -138,7 +140,7 @@ def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] - self.cp.update(int(sec_since_boot() * 1e9), False) + self.cp.update(int(sec_since_boot() * 1e9), True) self.CS.update(self.cp) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 83e90c3c7d7fcb..0c27dc2c7c7268 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -57,6 +57,8 @@ def get_params(candidate, fingerprint): ret.carFingerprint = candidate ret.enableCruise = False + ret.steerMPCReactTime = 0.025 # increase total MPC projected time by 25 ms + ret.steerMPCDampTime = 0.25 # dampen desired angle over 250ms (5 mpc cycles) # Presence of a camera on the object bus is ok. # Have to go passive if ASCM is online (ACC-enabled cars), @@ -196,7 +198,7 @@ def get_params(candidate, fingerprint): # returns a car.CarState def update(self, c): - self.pt_cp.update(int(sec_since_boot() * 1e9), False) + self.pt_cp.update(int(sec_since_boot() * 1e9), True) self.CS.update(self.pt_cp) # create message diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index f4c7cade8c0b99..8aa54da1f473f5 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -89,7 +89,9 @@ def get_can_signals(CP): ("STANDSTILL", 50)] if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH): - signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)] + signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1), + ("LEAD_DISTANCE", "RADAR_HUD", 0)] + checks += [("RADAR_HUD", 50)] else: signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1), ("DOOR_OPEN_FR", "DOORS_STATUS", 1), @@ -153,6 +155,10 @@ def __init__(self, CP): self.cruise_setting = 0 self.v_cruise_pcm_prev = 0 self.blinker_on = 0 + self.prev_steering_counter = 0 + self.steer_data_reused = 0 + self.steer_good_count = 0 + self.steer_data_skipped = 0 self.left_blinker_on = 0 self.right_blinker_on = 0 @@ -192,17 +198,18 @@ def update(self, cp, cp_cam): if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH): # TODO: find wheels moving bit in dbc self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'] + self.lead_distance= cp.vl["RADAR_HUD"]['LEAD_DISTANCE'] else: self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] self.door_all_closed = not any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']]) self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED'] - # 2 = temporary; 3 = TBD; 4 = temporary, hit a bump; 5 = (permanent); 6 = temporary; 7 = (permanent) + # 2 = temporary; 3 = TBD; 4 = ignore, steer override or bump; 5 = (permanent); 6 = temporary; 7 = (permanent) # TODO: Use values from DBC to parse this field self.steer_error = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 2, 3, 4, 6] - self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] != 0 - self.steer_warning = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 3] # 3 is low speed lockout, not worth a warning + self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 4] + self.steer_warning = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 3, 4] # 3 is low speed lockout, not worth a warning if self.CP.radarOffCan: self.brake_error = 0 else: @@ -239,6 +246,16 @@ def update(self, cp, cp_cam): self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR'] self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] + steer_counter = cp.vl["STEERING_SENSORS"]['COUNTER'] + if not (steer_counter == (self.prev_steering_counter + 1) % 4): + if steer_counter == self.prev_steering_counter: + self.steer_data_reused += 1 + print("data reused: %d skipped %d good %d %d vs %d" % (self.steer_data_reused, self.steer_data_skipped, self.steer_good_count, steer_counter, (self.prev_steering_counter + 1) % 4)) + else: + self.steer_data_skipped += 1 + else: + self.steer_good_count += 1 + self.prev_steering_counter = steer_counter self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS'] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 3acd2c7415345d..4dee7533ab34e2 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -175,6 +175,8 @@ def get_params(candidate, fingerprint): rotationalInertia_civic = 2500 tireStiffnessFront_civic = 192150 tireStiffnessRear_civic = 202500 + ret.steerMPCReactTime = 0.025 # increase total MPC projected time by 25 ms + ret.steerMPCDampTime = 0.25 # dampen desired angle over 250ms (5 mpc cycles) # Optimized car params: tire_stiffness_factor and steerRatio are a result of a vehicle # model optimization process. Certain Hondas have an extra steering sensor at the bottom @@ -357,7 +359,7 @@ def get_params(candidate, fingerprint): ret.startAccel = 0.5 ret.steerActuatorDelay = 0.1 - ret.steerRateCost = 0.5 + ret.steerRateCost = 0.4 return ret @@ -366,7 +368,7 @@ def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] - self.cp.update(int(sec_since_boot() * 1e9), False) + self.cp.update(int(sec_since_boot() * 1e9), True) self.cp_cam.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.cp, self.cp_cam) @@ -481,6 +483,8 @@ def update(self, c): events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 + if self.can_invalid_count > 0: + print( " can_invalid_count = %d" % (self.can_invalid_count)) if not self.CS.cam_can_valid and self.CP.enableCamera: self.cam_can_invalid_count += 1 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index bd3e63740c1646..3d29032a21e364 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -185,7 +185,6 @@ def update(self, cp, cp_cam): self.standstill = not v_wheel > 0.1 self.angle_steers = cp.vl["SAS11"]['SAS_Angle'] - self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed'] self.yaw_rate = cp.vl["ESP12"]['YAW_RATE'] self.main_on = True self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index d7fc2bd394774e..84a1c418ffcc86 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -68,6 +68,8 @@ def get_params(candidate, fingerprint): rotationalInertia_civic = 2500 tireStiffnessFront_civic = 192150 tireStiffnessRear_civic = 202500 + ret.steerMPCReactTime = 0.025 # increase total MPC projected time by 25 ms + ret.steerMPCDampTime = 0.25 # dampen desired angle over 250ms (5 mpc cycles) ret.steerActuatorDelay = 0.1 # Default delay tire_stiffness_factor = 1. @@ -179,7 +181,7 @@ def get_params(candidate, fingerprint): def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] - self.cp.update(int(sec_since_boot() * 1e9), False) + self.cp.update(int(sec_since_boot() * 1e9), True) self.cp_cam.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.cp, self.cp_cam) # create message diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 3a5e05257c4a90..d86a959f8a1613 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -61,6 +61,8 @@ def get_params(candidate, fingerprint): ret.tireStiffnessFront = 1e6 # very stiff to neglect slip ret.tireStiffnessRear = 1e6 # very stiff to neglect slip ret.steerRatioRear = 0. + ret.steerMPCReactTime = 0.025 # increase total MPC projected time by 25 ms + ret.steerMPCDampTime = 0.25 # dampen desired angle over 250ms (5 mpc cycles) ret.steerMaxBP = [0.] ret.steerMaxV = [0.] # 2/3rd torque allowed above 45 kph diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 93a1ac62b2ddda..8e1d0773e536db 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -50,6 +50,8 @@ def get_params(candidate, fingerprint): ret.enableCruise = False ret.steerLimitAlert = True ret.enableCamera = True + ret.steerMPCReactTime = 0.025 # increase total MPC projected time by 25 ms + ret.steerMPCDampTime = 0.25 # dampen desired angle over 250ms (5 mpc cycles) std_cargo = 136 ret.steerRateCost = 0.7 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 02debc47bf8484..7e6574d31da656 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -168,5 +168,6 @@ def update(self, cp, cp_cam): self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed) if self.CP.carFingerprint == CAR.PRIUS: self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 + self.angle_steers += self.angle_steers_rate * 0.1 else: self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM']) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8b6cb0a0129844..56da03fc71ce77 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -72,6 +72,8 @@ def get_params(candidate, fingerprint): rotationalInertia_civic = 2500 tireStiffnessFront_civic = 192150 tireStiffnessRear_civic = 202500 + ret.steerMPCReactTime = 0.025 # increase total MPC projected time by 25 ms + ret.steerMPCDampTime = 0.25 # dampen desired angle over 250ms (5 mpc cycles) ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay @@ -219,7 +221,7 @@ def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] - self.cp.update(int(sec_since_boot() * 1e9), False) + self.cp.update(int(sec_since_boot() * 1e9), True) # run the cam can update for 10s as we just need to know if the camera is alive if self.frame < 1000: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b5d699bdaabfa4..3c8d9c8ca34847 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -39,13 +39,19 @@ def isEnabled(state): return (isActive(state) or state == State.preEnabled) -def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor, +def data_sample(CI, CC, CS, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor, poller, cal_status, cal_perc, overtemp, free_space, low_battery, - driver_status, state, mismatch_counter, params, plan, path_plan): + driver_status, state, mismatch_counter, rk, params, plan, path_plan): """Receive data from sockets and create events for battery, temperature and disk space""" + rk.monitor_time() # Update carstate from CAN and create events - CS = CI.update(CC) + if rk.remaining > 10. / 1000 or rk.frame < 1000: + CS = CI.update(CC) + else: + CS = CS + print("CAN lagging!", rk.remaining, rk.frame) + events = list(CS.events) enabled = isEnabled(state) @@ -257,8 +263,7 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) # Steering PID loop and lateral MPC - actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, - CS.steeringPressed, CP, VM, path_plan) + actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringPressed, CP, VM, path_plan) # Send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: @@ -336,7 +341,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis "vEgo": CS.vEgo, "vEgoRaw": CS.vEgoRaw, "angleSteers": CS.steeringAngle, - "curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo), + "curvature": VM.calc_curvature((LaC.dampened_actual_angle - path_plan.pathPlan.angleOffset) * CV.DEG_TO_RAD, CS.vEgo), "steerOverride": CS.steeringPressed, "state": state, "engageable": not bool(get_events(events, [ET.NO_ENTRY])), @@ -346,7 +351,10 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis "upAccelCmd": float(LoC.pid.p), "uiAccelCmd": float(LoC.pid.i), "ufAccelCmd": float(LoC.pid.f), - "angleSteersDes": float(LaC.angle_steers_des), + "angleSteersDes": float(path_plan.pathPlan.angleSteers), + "dampAngleSteersDes": float(LaC.dampened_desired_angle), + "rateModeFF": LaC.rate_mode, + "angleModeFF": LaC.angle_mode, "upSteer": float(LaC.pid.p), "uiSteer": float(LaC.pid.i), "ufSteer": float(LaC.pid.f), @@ -387,7 +395,7 @@ def controlsd_thread(gctx=None, rate=100): gc.disable() # start the loop - set_realtime_priority(3) + set_realtime_priority(4) context = zmq.Context() params = Params() @@ -459,7 +467,7 @@ def controlsd_thread(gctx=None, rate=100): path_plan = messaging.new_message() path_plan.init('pathPlan') - rk = Ratekeeper(rate, print_delay_threshold=2. / 1000) + rk = Ratekeeper(rate, print_delay_threshold=12. / 1000) controls_params = params.get("ControlsParams") # Read angle offset from previous drive @@ -472,6 +480,7 @@ def controlsd_thread(gctx=None, rate=100): pass prof = Profiler(False) # off by default + CS = None while True: start_time = int(sec_since_boot() * 1e9) @@ -479,9 +488,10 @@ def controlsd_thread(gctx=None, rate=100): # Sample data and compute car events CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan =\ - data_sample(CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor, + data_sample(CI, CC, CS, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, - state, mismatch_counter, params, plan, path_plan) + state, mismatch_counter, rk, params, plan, path_plan) + prof.checkpoint("Sample") path_plan_age = (start_time - path_plan.logMonoTime) / 1e9 @@ -515,7 +525,6 @@ def controlsd_thread(gctx=None, rate=100): live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, params, v_acc, a_acc) prof.checkpoint("Sent") - rk.keep_time() # Run at 100Hz prof.display() diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 69494387083414..9f801324f40c1b 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,48 +1,135 @@ +import numpy as np +from common.realtime import sec_since_boot from selfdrive.controls.lib.pid import PIController from common.numpy_fast import interp +from selfdrive.kegman_conf import kegman_conf from cereal import car _DT = 0.01 # 100Hz -_DT_MPC = 0.05 # 20Hz def get_steer_max(CP, v_ego): return interp(v_ego, CP.steerMaxBP, CP.steerMaxV) - class LatControl(object): def __init__(self, CP): - self.pid = PIController((CP.steerKpBP, CP.steerKpV), - (CP.steerKiBP, CP.steerKiV), + + kegman = kegman_conf(CP) + self.gernbySteer = True + self.mpc_frame = 0 + self.total_desired_projection = max(0.0, CP.steerMPCReactTime + CP.steerMPCDampTime) + self.desired_smoothing = max(1.0, CP.steerMPCDampTime / _DT) + self.angle_ff_gain = 2.0 + self.rate_ff_gain = 0.2 + self.angle_ff_bp = [[0.5, 5.0],[0.0, 1.0]] + self.dampened_actual_angle = 0.0 + self.dampened_desired_angle = 0.0 + self.dampened_desired_rate = 0.0 + self.rate_mode = 0.0 + self.angle_mode = 0.0 + self.angle_ff_counter = 0 + self.starting_integral = 0.0 + self.last_angle = 0.0 + + KpV = [interp(25.0, CP.steerKpBP, CP.steerKpV)] + KiV = [interp(25.0, CP.steerKiBP, CP.steerKiV)] + self.pid = PIController(([0.], KpV), + ([0.], KiV), k_f=CP.steerKf, pos_limit=1.0) - self.last_cloudlog_t = 0.0 - self.angle_steers_des = 0. + + def adjust_angle_gain(self): + if self.angle_ff_counter > 100: + if abs(self.dampened_desired_angle) > abs(self.last_angle): + if self.pid.i > self.starting_integral: + if self.pid.f > 0: + self.angle_ff_gain *= 1.01 + else: + self.angle_ff_gain *= 0.99 + elif self.pid.i < self.starting_integral: + if self.pid.f < 0: + self.angle_ff_gain *= 1.01 + else: + self.angle_ff_gain *= 0.99 + self.angle_ff_counter = 0 + self.last_angle = self.dampened_desired_angle + self.starting_integral = self.pid.i + elif self.angle_ff_counter == 0: + self.starting_integral = self.pid.i + self.last_angle = self.dampened_desired_angle + self.angle_ff_counter += 1 + + def live_tune(self, CP): + self.mpc_frame += 1 + if self.mpc_frame % 300 == 0: + # live tuning through /data/openpilot/tune.py overrides interface.py settings + kegman = kegman_conf() + if kegman.conf['tuneGernby'] == "1": + self.steerKpV = np.array([float(kegman.conf['Kp'])]) + self.steerKiV = np.array([float(kegman.conf['Ki'])]) + self.total_desired_projection = max(0.0, float(kegman.conf['dampMPC']) + float(kegman.conf['reactMPC'])) + self.desired_smoothing = max(1.0, float(kegman.conf['dampMPC']) / _DT) + self.gernbySteer = (self.total_desired_projection > 0 or self.desired_smoothing > 1) + self.angle_ff_gain = float(kegman.conf['angleFF']) + self.rate_ff_gain = float(kegman.conf['rateFF']) + + # Eliminate break-points, since they aren't needed (and would cause problems for resonance) + KpV = [interp(25.0, CP.steerKpBP, self.steerKpV)] + KiV = [interp(25.0, CP.steerKiBP, self.steerKiV)] + self.pid._k_i = ([0.], KiV) + self.pid._k_p = ([0.], KpV) + print(self.angle_ff_gain, self.rate_ff_gain, self.total_desired_projection, self.desired_smoothing, self.gernbySteer) + else: + self.gernbySteer = False + self.mpc_frame = 0 + def reset(self): self.pid.reset() def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan): + + self.live_tune(CP) + if v_ego < 0.3 or not active: output_steer = 0.0 self.pid.reset() + self.dampened_desired_angle = float(angle_steers) else: - # TODO: ideally we should interp, but for tuning reasons we keep the mpc solution - # constant for 0.05s. - #dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps - #self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) - self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner - - steers_max = get_steer_max(CP, v_ego) - self.pid.pos_limit = steers_max - self.pid.neg_limit = -steers_max - steer_feedforward = self.angle_steers_des # feedforward desired angle + if self.gernbySteer == False: + self.dampened_desired_angle = float(path_plan.angleSteers) + + else: + cur_time = sec_since_boot() + projected_desired_angle = interp(cur_time + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcAngles) + self.dampened_desired_angle += ((projected_desired_angle - self.dampened_desired_angle) / self.desired_smoothing) + projected_desired_rate = interp(cur_time + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcRates) + self.dampened_desired_rate += ((projected_desired_rate - self.dampened_desired_rate) / self.desired_smoothing) + if CP.steerControlType == car.CarParams.SteerControlType.torque: - # TODO: feedforward something based on path_plan.rateSteers - steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque - steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) - deadzone = 0.0 - output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, - feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) + steers_max = get_steer_max(CP, v_ego) + self.pid.pos_limit = steers_max + self.pid.neg_limit = -steers_max + deadzone = 0.0 + + ff_target_angle = self.dampened_desired_angle - path_plan.angleOffset + self.angle_mode = interp(abs(ff_target_angle), self.angle_ff_bp[0], self.angle_ff_bp[1]) + self.rate_mode = 1.0 - self.angle_mode + angle_feedforward = self.angle_mode * self.angle_ff_gain * ff_target_angle + rate_feedforward = self.rate_mode * self.rate_ff_gain * self.dampened_desired_rate + feed_forward = v_ego**2 * (rate_feedforward + angle_feedforward) + + output_steer = self.pid.update(self.dampened_desired_angle, angle_steers, check_saturation=(v_ego > 10), + override=steer_override, feedforward=feed_forward, speed=v_ego, deadzone=deadzone) + + #if not steer_override and abs(angle_steers) > (self.angle_ff_bp[0][1] / 2.0): + # self.adjust_angle_gain() + #else: + # self.angle_ff_counter = 0 self.sat_flag = self.pid.saturated - return output_steer, float(self.angle_steers_des) + self.dampened_actual_angle += 0.1 * (angle_steers - self.dampened_actual_angle) + + if CP.steerControlType == car.CarParams.SteerControlType.torque: + return float(output_steer), float(path_plan.angleSteers) + else: + return float(self.dampened_desired_angle), float(path_plan.angleSteers) diff --git a/selfdrive/controls/lib/latcontrol_helpers.py b/selfdrive/controls/lib/latcontrol_helpers.py index 90abcebf10bf53..eb08d3859ea5f4 100644 --- a/selfdrive/controls/lib/latcontrol_helpers.py +++ b/selfdrive/controls/lib/latcontrol_helpers.py @@ -17,13 +17,8 @@ def calc_d_lookahead(v_ego, d_poly): # howfar we look ahead is function of speed and how much curvy is the path offset_lookahead = 1. k_lookahead = 7. - # integrate abs value of second derivative of poly to get a measure of path curvature - pts_len = 50. # m - if len(d_poly) > 0: - pts = np.polyval([6 * d_poly[0], 2 * d_poly[1]], np.arange(0, pts_len)) - else: - pts = 0. - curv = np.sum(np.abs(pts)) / pts_len + + curv = abs(calc_poly_curvature(d_poly)) k_curv = interp(curv, _K_CURV_BP, _K_CURV_V) @@ -33,6 +28,15 @@ def calc_d_lookahead(v_ego, d_poly): d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv return d_lookahead +def calc_poly_curvature(line_poly): + # integrate abs value of second derivative of poly to get a measure of path curvature + pts_len = 50. # m + if len(line_poly) > 0: + pts = np.polyval([6 * line_poly[0], 2 * line_poly[1]], np.arange(0, pts_len)) + else: + pts = 0. + return np.sum((pts)) / pts_len + def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset): # this function returns the lateral offset given the steering angle, speed and the lookahead distance diff --git a/selfdrive/controls/lib/model_parser.py b/selfdrive/controls/lib/model_parser.py index 747e918f750a86..1a4741f03b25b1 100644 --- a/selfdrive/controls/lib/model_parser.py +++ b/selfdrive/controls/lib/model_parser.py @@ -1,4 +1,5 @@ -from common.numpy_fast import interp +import math +from common.numpy_fast import interp, clip from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv CAMERA_OFFSET = 0.06 # m from center car to camera @@ -8,7 +9,6 @@ class ModelParser(object): def __init__(self): self.d_poly = [0., 0., 0., 0.] self.c_poly = [0., 0., 0., 0.] - self.c_prob = 0. self.last_model = 0. self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 self._path_pinv = compute_path_pinv() @@ -18,8 +18,17 @@ def __init__(self): self.lane_width = 3.7 self.l_prob = 0. self.r_prob = 0. + self.lane_prob= 0. - def update(self, v_ego, md): + def fix_polys(self, winner_points, path_points): + step_size = winner_points[1] - winner_points[0] + path_points[0] = clip(path_points[0], winner_points[0] - self.lane_width / 2.0, winner_points[0] + self.lane_width / 2.0) + for i in range(1,50): + winner_points[i] = winner_points[i-1] + step_size + path_points[i] = path_points[i-1] + step_size + return model_polyfit(winner_points, self._path_pinv), model_polyfit(path_points, self._path_pinv) + + def update(self, v_ego, md, v_curv=0.0): if md is not None: p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line @@ -43,9 +52,28 @@ def update(self, v_ego, md): (1 - self.lane_width_certainty) * speed_lane_width lane_width_diff = abs(self.lane_width - current_lane_width) - lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0]) + lane_prob = interp(lane_width_diff, [0.3, interp(v_ego, [20.0, 25.0], [1.0, 0.4])], [1.0, 0.0]) + + if (abs(v_curv) < 0.0005 and l_prob > 0.5 and r_prob > 0.5 and v_ego > 22.0) or self.lane_prob == 0.0: + steer_compensation = 1.2 * v_curv * v_ego + total_left_divergence = (md.model.leftLane.points[5] - md.model.leftLane.points[0]) * r_prob + steer_compensation + total_right_divergence = -((md.model.rightLane.points[5] - md.model.rightLane.points[0]) * l_prob + steer_compensation) - r_prob *= lane_r_prob + if (total_left_divergence > abs(total_right_divergence) \ + and (self.lane_prob > 0 or self.r_prob > 0)) or (self.lane_prob == 0 and self.l_prob == 0): + l_prob *= lane_prob + if lane_prob == 0.0: + p_prob = 0.5 + #r_prob *= 1.5 + r_poly, p_poly = self.fix_polys(map(float, md.model.rightLane.points), map(float, md.model.path.points)) + elif (total_right_divergence > abs(total_left_divergence)) \ + or (self.lane_prob == 0 and self.r_prob == 0): + r_prob *= lane_prob + if lane_prob == 0.0: + p_prob = 0.5 + #l_prob *= 1.5 + l_poly, p_poly = self.fix_polys(map(float, md.model.leftLane.points), map(float, md.model.path.points)) + self.lane_prob = lane_prob self.lead_dist = md.model.lead.dist self.lead_prob = md.model.lead.prob diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 0161d2f94633a0..b60ba84b78ef10 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -10,6 +10,7 @@ from selfdrive.controls.lib.model_parser import ModelParser import selfdrive.messaging as messaging +_DT_MPC = 0.05 def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): states[0].x = v_ego * delay @@ -29,6 +30,9 @@ def __init__(self, CP): self.setup_mpc(CP.steerRateCost) self.invalid_counter = 0 + self.mpc_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + self.mpc_rates = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + self.mpc_times = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -48,15 +52,16 @@ def setup_mpc(self, steer_rate_cost): def update(self, CP, VM, CS, md, live100, live_parameters): v_ego = CS.carState.vEgo - angle_steers = CS.carState.steeringAngle + angle_steers = live100.live100.angleSteers + v_curv = live100.live100.curvature active = live100.live100.active angle_offset_bias = live100.live100.angleModelBias + live_parameters.liveParameters.angleOffsetAverage - self.MP.update(v_ego, md) + self.MP.update(v_ego, md, v_curv) # Run MPC - self.angle_steers_des_prev = self.angle_steers_des_mpc + self.angle_steers_des_prev = live100.live100.dampAngleSteersDes VM.update_params(live_parameters.liveParameters.stiffnessFactor, live_parameters.liveParameters.steerRatio) curvature_factor = VM.curvature_factor(v_ego) @@ -65,6 +70,7 @@ def update(self, CP, VM, CS, md, live100, live_parameters): p_poly = libmpc_py.ffi.new("double[4]", list(self.MP.p_poly)) # account for actuation delay + self.cur_state[0].delta = math.radians(live100.live100.dampAngleSteersDes - angle_offset_bias) / VM.sR self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed @@ -74,6 +80,13 @@ def update(self, CP, VM, CS, md, live100, live_parameters): # reset to current steer angle if not active or overriding if active: + self.mpc_angles[0] = live100.live100.dampAngleSteersDes + self.mpc_times[0] = live100.logMonoTime * 1e-9 + for i in range(1,20): + self.mpc_times[i] = self.mpc_times[i-1] + _DT_MPC + self.mpc_angles[i] = float(math.degrees(self.mpc_solution[0].delta[i] * VM.sR) + angle_offset_bias) + self.mpc_rates[i-1] = (self.mpc_angles[i] - self.mpc_angles[i-1]) / (self.mpc_times[i] - self.mpc_times[i-1]) + delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: @@ -84,7 +97,6 @@ def update(self, CP, VM, CS, md, live100, live_parameters): self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset_bias) - # Check for infeasable MPC solution mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() @@ -116,6 +128,10 @@ def update(self, CP, VM, CS, md, live100, live_parameters): plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = float(live_parameters.liveParameters.angleOffsetAverage) + plan_send.pathPlan.mpcAngles = map(float, self.mpc_angles) + plan_send.pathPlan.mpcRates = map(float, self.mpc_rates) + plan_send.pathPlan.mpcTimes = map(float, self.mpc_times) + plan_send.pathPlan.laneProb =float(self.MP.lane_prob) plan_send.pathPlan.valid = bool(plan_valid) plan_send.pathPlan.paramsValid = bool(live_parameters.liveParameters.valid) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 4d44c96b9dd34e..9484e5e3cb9a63 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -132,6 +132,7 @@ def radard_thread(gctx=None): if l100 is not None: active = l100.live100.active v_ego = l100.live100.vEgo + v_curv = l100.live100.curvature steer_angle = l100.live100.angleSteers steer_override = l100.live100.steerOverride @@ -147,7 +148,7 @@ def radard_thread(gctx=None): last_md_ts = md.logMonoTime # *** get path prediction from the model *** - MP.update(v_ego, md) + MP.update(v_ego, md, v_curv) # run kalman filter only if prob is high enough if MP.lead_prob > 0.7: diff --git a/selfdrive/kegman_conf.py b/selfdrive/kegman_conf.py new file mode 100644 index 00000000000000..77343cf0fd4fa0 --- /dev/null +++ b/selfdrive/kegman_conf.py @@ -0,0 +1,93 @@ +import json +import os + +class kegman_conf(): + def __init__(self, CP=None): + self.conf = self.read_config() + if CP is not None: + self.init_config(CP) + + def init_config(self, CP): + write_conf = False + if self.conf['tuneGernby'] == "-1": + self.conf['tuneGernby'] = str(1) + write_conf = True + if self.conf['rateFF'] == "-1": + self.conf['rateFF'] = "0.1" + write_conf = True + if self.conf['angleFF'] == "-1": + self.conf['angleFF'] = "1.0" + write_conf = True + if self.conf['reactMPC'] == "-1": + self.conf['reactMPC'] = str(round(CP.steerMPCReactTime,3)) + write_conf = True + if self.conf['dampMPC'] == "-1": + self.conf['dampMPC'] = str(round(CP.steerMPCDampTime,3)) + write_conf = True + if self.conf['Kp'] == "-1": + self.conf['Kp'] = str(round(CP.steerKpV[0],3)) + write_conf = True + if self.conf['Ki'] == "-1": + self.conf['Ki'] = str(round(CP.steerKiV[0],3)) + write_conf = True + + if write_conf: + self.write_config(self.config) + + def read_config(self): + self.element_updated = False + + if os.path.isfile('/data/kegman.json'): + with open('/data/kegman.json', 'r') as f: + self.config = json.load(f) + + if "battPercOff" not in self.config: + self.config.update({"battPercOff":"25"}) + self.config.update({"carVoltageMinEonShutdown":"11800"}) + self.config.update({"brakeStoppingTarget":"0.25"}) + self.element_updated = True + + if "tuneGernby" not in self.config: + self.config.update({"tuneGernby":"0"}) + self.config.update({"react":"-1"}) + self.config.update({"Kp":"-1"}) + self.config.update({"Ki":"-1"}) + self.element_updated = True + + if "rateFF" not in self.config: + self.config.update({"rateFF":"0.1"}) + self.config.update({"angleFF":"1.0"}) + self.element_updated = True + + if "dampMPC" not in self.config: + self.config.update({"dampMPC":"-1"}) + self.config.update({"dampSteer":"-1"}) + self.element_updated = True + + if "reactMPC" not in self.config: + self.config.update({"reactMPC":"-1"}) + self.config.update({"reactSteer":"-1"}) + self.element_updated = True + + # Force update battery charge limits to higher values for Big Model + #if self.config['battChargeMin'] != "75": + # self.config.update({"battChargeMin":"75"}) + # self.config.update({"battChargeMax":"80"}) + # self.element_updated = True + + if self.element_updated: + self.write_config(self.config) + + else: + self.config = {"cameraOffset":"0.06", "lastTrMode":"1", "battChargeMin":"60", "battChargeMax":"70", \ + "wheelTouchSeconds":"180", "battPercOff":"25", "carVoltageMinEonShutdown":"11800", \ + "brakeStoppingTarget":"0.25", "tuneGernby":"0", "reactSteer":"-1", "reactMPC":"-1", \ + "dampMPC":"-1", "dampSteer":"-1", "rateFF":"0.1", "angleFF":"1.0", "Kp":"-1", "Ki":"-1"} + + self.write_config(self.config) + return self.config + + def write_config(self, config): + with open('/data/kegman.json', 'w') as f: + json.dump(self.config, f, indent=2, sort_keys=True) + os.chmod("/data/kegman.json", 0o764) diff --git a/selfdrive/locationd/kalman/loc_local.cpp b/selfdrive/locationd/kalman/loc_local.cpp new file mode 100644 index 00000000000000..b8f4acaa2d0428 --- /dev/null +++ b/selfdrive/locationd/kalman/loc_local.cpp @@ -0,0 +1,2922 @@ +#define _CFFI_ + +/* We try to define Py_LIMITED_API before including Python.h. + + Mess: we can only define it if Py_DEBUG, Py_TRACE_REFS and + Py_REF_DEBUG are not defined. This is a best-effort approximation: + we can learn about Py_DEBUG from pyconfig.h, but it is unclear if + the same works for the other two macros. Py_DEBUG implies them, + but not the other way around. + + Issue #350 is still open: on Windows, the code here causes it to link + with PYTHON36.DLL (for example) instead of PYTHON3.DLL. A fix was + attempted in 164e526a5515 and 14ce6985e1c3, but reverted: virtualenv + does not make PYTHON3.DLL available, and so the "correctly" compiled + version would not run inside a virtualenv. We will re-apply the fix + after virtualenv has been fixed for some time. For explanation, see + issue #355. For a workaround if you want PYTHON3.DLL and don't worry + about virtualenv, see issue #350. See also 'py_limited_api' in + setuptools_ext.py. +*/ +#if !defined(_CFFI_USE_EMBEDDING) && !defined(Py_LIMITED_API) +# include +# if !defined(Py_DEBUG) && !defined(Py_TRACE_REFS) && !defined(Py_REF_DEBUG) +# define Py_LIMITED_API +# endif +#endif + +#include +#ifdef __cplusplus +extern "C" { +#endif +#include + +/* This part is from file 'cffi/parse_c_type.h'. It is copied at the + beginning of C sources generated by CFFI's ffi.set_source(). */ + +typedef void *_cffi_opcode_t; + +#define _CFFI_OP(opcode, arg) (_cffi_opcode_t)(opcode | (((uintptr_t)(arg)) << 8)) +#define _CFFI_GETOP(cffi_opcode) ((unsigned char)(uintptr_t)cffi_opcode) +#define _CFFI_GETARG(cffi_opcode) (((intptr_t)cffi_opcode) >> 8) + +#define _CFFI_OP_PRIMITIVE 1 +#define _CFFI_OP_POINTER 3 +#define _CFFI_OP_ARRAY 5 +#define _CFFI_OP_OPEN_ARRAY 7 +#define _CFFI_OP_STRUCT_UNION 9 +#define _CFFI_OP_ENUM 11 +#define _CFFI_OP_FUNCTION 13 +#define _CFFI_OP_FUNCTION_END 15 +#define _CFFI_OP_NOOP 17 +#define _CFFI_OP_BITFIELD 19 +#define _CFFI_OP_TYPENAME 21 +#define _CFFI_OP_CPYTHON_BLTN_V 23 // varargs +#define _CFFI_OP_CPYTHON_BLTN_N 25 // noargs +#define _CFFI_OP_CPYTHON_BLTN_O 27 // O (i.e. a single arg) +#define _CFFI_OP_CONSTANT 29 +#define _CFFI_OP_CONSTANT_INT 31 +#define _CFFI_OP_GLOBAL_VAR 33 +#define _CFFI_OP_DLOPEN_FUNC 35 +#define _CFFI_OP_DLOPEN_CONST 37 +#define _CFFI_OP_GLOBAL_VAR_F 39 +#define _CFFI_OP_EXTERN_PYTHON 41 + +#define _CFFI_PRIM_VOID 0 +#define _CFFI_PRIM_BOOL 1 +#define _CFFI_PRIM_CHAR 2 +#define _CFFI_PRIM_SCHAR 3 +#define _CFFI_PRIM_UCHAR 4 +#define _CFFI_PRIM_SHORT 5 +#define _CFFI_PRIM_USHORT 6 +#define _CFFI_PRIM_INT 7 +#define _CFFI_PRIM_UINT 8 +#define _CFFI_PRIM_LONG 9 +#define _CFFI_PRIM_ULONG 10 +#define _CFFI_PRIM_LONGLONG 11 +#define _CFFI_PRIM_ULONGLONG 12 +#define _CFFI_PRIM_FLOAT 13 +#define _CFFI_PRIM_DOUBLE 14 +#define _CFFI_PRIM_LONGDOUBLE 15 + +#define _CFFI_PRIM_WCHAR 16 +#define _CFFI_PRIM_INT8 17 +#define _CFFI_PRIM_UINT8 18 +#define _CFFI_PRIM_INT16 19 +#define _CFFI_PRIM_UINT16 20 +#define _CFFI_PRIM_INT32 21 +#define _CFFI_PRIM_UINT32 22 +#define _CFFI_PRIM_INT64 23 +#define _CFFI_PRIM_UINT64 24 +#define _CFFI_PRIM_INTPTR 25 +#define _CFFI_PRIM_UINTPTR 26 +#define _CFFI_PRIM_PTRDIFF 27 +#define _CFFI_PRIM_SIZE 28 +#define _CFFI_PRIM_SSIZE 29 +#define _CFFI_PRIM_INT_LEAST8 30 +#define _CFFI_PRIM_UINT_LEAST8 31 +#define _CFFI_PRIM_INT_LEAST16 32 +#define _CFFI_PRIM_UINT_LEAST16 33 +#define _CFFI_PRIM_INT_LEAST32 34 +#define _CFFI_PRIM_UINT_LEAST32 35 +#define _CFFI_PRIM_INT_LEAST64 36 +#define _CFFI_PRIM_UINT_LEAST64 37 +#define _CFFI_PRIM_INT_FAST8 38 +#define _CFFI_PRIM_UINT_FAST8 39 +#define _CFFI_PRIM_INT_FAST16 40 +#define _CFFI_PRIM_UINT_FAST16 41 +#define _CFFI_PRIM_INT_FAST32 42 +#define _CFFI_PRIM_UINT_FAST32 43 +#define _CFFI_PRIM_INT_FAST64 44 +#define _CFFI_PRIM_UINT_FAST64 45 +#define _CFFI_PRIM_INTMAX 46 +#define _CFFI_PRIM_UINTMAX 47 +#define _CFFI_PRIM_FLOATCOMPLEX 48 +#define _CFFI_PRIM_DOUBLECOMPLEX 49 +#define _CFFI_PRIM_CHAR16 50 +#define _CFFI_PRIM_CHAR32 51 + +#define _CFFI__NUM_PRIM 52 +#define _CFFI__UNKNOWN_PRIM (-1) +#define _CFFI__UNKNOWN_FLOAT_PRIM (-2) +#define _CFFI__UNKNOWN_LONG_DOUBLE (-3) + +#define _CFFI__IO_FILE_STRUCT (-1) + + +struct _cffi_global_s { + const char *name; + void *address; + _cffi_opcode_t type_op; + void *size_or_direct_fn; // OP_GLOBAL_VAR: size, or 0 if unknown + // OP_CPYTHON_BLTN_*: addr of direct function +}; + +struct _cffi_getconst_s { + unsigned long long value; + const struct _cffi_type_context_s *ctx; + int gindex; +}; + +struct _cffi_struct_union_s { + const char *name; + int type_index; // -> _cffi_types, on a OP_STRUCT_UNION + int flags; // _CFFI_F_* flags below + size_t size; + int alignment; + int first_field_index; // -> _cffi_fields array + int num_fields; +}; +#define _CFFI_F_UNION 0x01 // is a union, not a struct +#define _CFFI_F_CHECK_FIELDS 0x02 // complain if fields are not in the + // "standard layout" or if some are missing +#define _CFFI_F_PACKED 0x04 // for CHECK_FIELDS, assume a packed struct +#define _CFFI_F_EXTERNAL 0x08 // in some other ffi.include() +#define _CFFI_F_OPAQUE 0x10 // opaque + +struct _cffi_field_s { + const char *name; + size_t field_offset; + size_t field_size; + _cffi_opcode_t field_type_op; +}; + +struct _cffi_enum_s { + const char *name; + int type_index; // -> _cffi_types, on a OP_ENUM + int type_prim; // _CFFI_PRIM_xxx + const char *enumerators; // comma-delimited string +}; + +struct _cffi_typename_s { + const char *name; + int type_index; /* if opaque, points to a possibly artificial + OP_STRUCT which is itself opaque */ +}; + +struct _cffi_type_context_s { + _cffi_opcode_t *types; + const struct _cffi_global_s *globals; + const struct _cffi_field_s *fields; + const struct _cffi_struct_union_s *struct_unions; + const struct _cffi_enum_s *enums; + const struct _cffi_typename_s *typenames; + int num_globals; + int num_struct_unions; + int num_enums; + int num_typenames; + const char *const *includes; + int num_types; + int flags; /* future extension */ +}; + +struct _cffi_parse_info_s { + const struct _cffi_type_context_s *ctx; + _cffi_opcode_t *output; + unsigned int output_size; + size_t error_location; + const char *error_message; +}; + +struct _cffi_externpy_s { + const char *name; + size_t size_of_result; + void *reserved1, *reserved2; +}; + +#ifdef _CFFI_INTERNAL +static int parse_c_type(struct _cffi_parse_info_s *info, const char *input); +static int search_in_globals(const struct _cffi_type_context_s *ctx, + const char *search, size_t search_len); +static int search_in_struct_unions(const struct _cffi_type_context_s *ctx, + const char *search, size_t search_len); +#endif + +/* this block of #ifs should be kept exactly identical between + c/_cffi_backend.c, cffi/vengine_cpy.py, cffi/vengine_gen.py + and cffi/_cffi_include.h */ +#if defined(_MSC_VER) +# include /* for alloca() */ +# if _MSC_VER < 1600 /* MSVC < 2010 */ + typedef __int8 int8_t; + typedef __int16 int16_t; + typedef __int32 int32_t; + typedef __int64 int64_t; + typedef unsigned __int8 uint8_t; + typedef unsigned __int16 uint16_t; + typedef unsigned __int32 uint32_t; + typedef unsigned __int64 uint64_t; + typedef __int8 int_least8_t; + typedef __int16 int_least16_t; + typedef __int32 int_least32_t; + typedef __int64 int_least64_t; + typedef unsigned __int8 uint_least8_t; + typedef unsigned __int16 uint_least16_t; + typedef unsigned __int32 uint_least32_t; + typedef unsigned __int64 uint_least64_t; + typedef __int8 int_fast8_t; + typedef __int16 int_fast16_t; + typedef __int32 int_fast32_t; + typedef __int64 int_fast64_t; + typedef unsigned __int8 uint_fast8_t; + typedef unsigned __int16 uint_fast16_t; + typedef unsigned __int32 uint_fast32_t; + typedef unsigned __int64 uint_fast64_t; + typedef __int64 intmax_t; + typedef unsigned __int64 uintmax_t; +# else +# include +# endif +# if _MSC_VER < 1800 /* MSVC < 2013 */ +# ifndef __cplusplus + typedef unsigned char _Bool; +# endif +# endif +#else +# include +# if (defined (__SVR4) && defined (__sun)) || defined(_AIX) || defined(__hpux) +# include +# endif +#endif + +#ifdef __GNUC__ +# define _CFFI_UNUSED_FN __attribute__((unused)) +#else +# define _CFFI_UNUSED_FN /* nothing */ +#endif + +#ifdef __cplusplus +# ifndef _Bool + typedef bool _Bool; /* semi-hackish: C++ has no _Bool; bool is builtin */ +# endif +#endif + +/********** CPython-specific section **********/ +#ifndef PYPY_VERSION + + +#if PY_MAJOR_VERSION >= 3 +# define PyInt_FromLong PyLong_FromLong +#endif + +#define _cffi_from_c_double PyFloat_FromDouble +#define _cffi_from_c_float PyFloat_FromDouble +#define _cffi_from_c_long PyInt_FromLong +#define _cffi_from_c_ulong PyLong_FromUnsignedLong +#define _cffi_from_c_longlong PyLong_FromLongLong +#define _cffi_from_c_ulonglong PyLong_FromUnsignedLongLong +#define _cffi_from_c__Bool PyBool_FromLong + +#define _cffi_to_c_double PyFloat_AsDouble +#define _cffi_to_c_float PyFloat_AsDouble + +#define _cffi_from_c_int(x, type) \ + (((type)-1) > 0 ? /* unsigned */ \ + (sizeof(type) < sizeof(long) ? \ + PyInt_FromLong((long)x) : \ + sizeof(type) == sizeof(long) ? \ + PyLong_FromUnsignedLong((unsigned long)x) : \ + PyLong_FromUnsignedLongLong((unsigned long long)x)) : \ + (sizeof(type) <= sizeof(long) ? \ + PyInt_FromLong((long)x) : \ + PyLong_FromLongLong((long long)x))) + +#define _cffi_to_c_int(o, type) \ + ((type)( \ + sizeof(type) == 1 ? (((type)-1) > 0 ? (type)_cffi_to_c_u8(o) \ + : (type)_cffi_to_c_i8(o)) : \ + sizeof(type) == 2 ? (((type)-1) > 0 ? (type)_cffi_to_c_u16(o) \ + : (type)_cffi_to_c_i16(o)) : \ + sizeof(type) == 4 ? (((type)-1) > 0 ? (type)_cffi_to_c_u32(o) \ + : (type)_cffi_to_c_i32(o)) : \ + sizeof(type) == 8 ? (((type)-1) > 0 ? (type)_cffi_to_c_u64(o) \ + : (type)_cffi_to_c_i64(o)) : \ + (Py_FatalError("unsupported size for type " #type), (type)0))) + +#define _cffi_to_c_i8 \ + ((int(*)(PyObject *))_cffi_exports[1]) +#define _cffi_to_c_u8 \ + ((int(*)(PyObject *))_cffi_exports[2]) +#define _cffi_to_c_i16 \ + ((int(*)(PyObject *))_cffi_exports[3]) +#define _cffi_to_c_u16 \ + ((int(*)(PyObject *))_cffi_exports[4]) +#define _cffi_to_c_i32 \ + ((int(*)(PyObject *))_cffi_exports[5]) +#define _cffi_to_c_u32 \ + ((unsigned int(*)(PyObject *))_cffi_exports[6]) +#define _cffi_to_c_i64 \ + ((long long(*)(PyObject *))_cffi_exports[7]) +#define _cffi_to_c_u64 \ + ((unsigned long long(*)(PyObject *))_cffi_exports[8]) +#define _cffi_to_c_char \ + ((int(*)(PyObject *))_cffi_exports[9]) +#define _cffi_from_c_pointer \ + ((PyObject *(*)(char *, struct _cffi_ctypedescr *))_cffi_exports[10]) +#define _cffi_to_c_pointer \ + ((char *(*)(PyObject *, struct _cffi_ctypedescr *))_cffi_exports[11]) +#define _cffi_get_struct_layout \ + not used any more +#define _cffi_restore_errno \ + ((void(*)(void))_cffi_exports[13]) +#define _cffi_save_errno \ + ((void(*)(void))_cffi_exports[14]) +#define _cffi_from_c_char \ + ((PyObject *(*)(char))_cffi_exports[15]) +#define _cffi_from_c_deref \ + ((PyObject *(*)(char *, struct _cffi_ctypedescr *))_cffi_exports[16]) +#define _cffi_to_c \ + ((int(*)(char *, struct _cffi_ctypedescr *, PyObject *))_cffi_exports[17]) +#define _cffi_from_c_struct \ + ((PyObject *(*)(char *, struct _cffi_ctypedescr *))_cffi_exports[18]) +#define _cffi_to_c_wchar_t \ + ((_cffi_wchar_t(*)(PyObject *))_cffi_exports[19]) +#define _cffi_from_c_wchar_t \ + ((PyObject *(*)(_cffi_wchar_t))_cffi_exports[20]) +#define _cffi_to_c_long_double \ + ((long double(*)(PyObject *))_cffi_exports[21]) +#define _cffi_to_c__Bool \ + ((_Bool(*)(PyObject *))_cffi_exports[22]) +#define _cffi_prepare_pointer_call_argument \ + ((Py_ssize_t(*)(struct _cffi_ctypedescr *, \ + PyObject *, char **))_cffi_exports[23]) +#define _cffi_convert_array_from_object \ + ((int(*)(char *, struct _cffi_ctypedescr *, PyObject *))_cffi_exports[24]) +#define _CFFI_CPIDX 25 +#define _cffi_call_python \ + ((void(*)(struct _cffi_externpy_s *, char *))_cffi_exports[_CFFI_CPIDX]) +#define _cffi_to_c_wchar3216_t \ + ((int(*)(PyObject *))_cffi_exports[26]) +#define _cffi_from_c_wchar3216_t \ + ((PyObject *(*)(int))_cffi_exports[27]) +#define _CFFI_NUM_EXPORTS 28 + +struct _cffi_ctypedescr; + +static void *_cffi_exports[_CFFI_NUM_EXPORTS]; + +#define _cffi_type(index) ( \ + assert((((uintptr_t)_cffi_types[index]) & 1) == 0), \ + (struct _cffi_ctypedescr *)_cffi_types[index]) + +static PyObject *_cffi_init(const char *module_name, Py_ssize_t version, + const struct _cffi_type_context_s *ctx) +{ + PyObject *module, *o_arg, *new_module; + void *raw[] = { + (void *)module_name, + (void *)version, + (void *)_cffi_exports, + (void *)ctx, + }; + + module = PyImport_ImportModule("_cffi_backend"); + if (module == NULL) + goto failure; + + o_arg = PyLong_FromVoidPtr((void *)raw); + if (o_arg == NULL) + goto failure; + + new_module = PyObject_CallMethod( + module, (char *)"_init_cffi_1_0_external_module", (char *)"O", o_arg); + + Py_DECREF(o_arg); + Py_DECREF(module); + return new_module; + + failure: + Py_XDECREF(module); + return NULL; +} + + +#ifdef HAVE_WCHAR_H +typedef wchar_t _cffi_wchar_t; +#else +typedef uint16_t _cffi_wchar_t; /* same random pick as _cffi_backend.c */ +#endif + +_CFFI_UNUSED_FN static uint16_t _cffi_to_c_char16_t(PyObject *o) +{ + if (sizeof(_cffi_wchar_t) == 2) + return (uint16_t)_cffi_to_c_wchar_t(o); + else + return (uint16_t)_cffi_to_c_wchar3216_t(o); +} + +_CFFI_UNUSED_FN static PyObject *_cffi_from_c_char16_t(uint16_t x) +{ + if (sizeof(_cffi_wchar_t) == 2) + return _cffi_from_c_wchar_t((_cffi_wchar_t)x); + else + return _cffi_from_c_wchar3216_t((int)x); +} + +_CFFI_UNUSED_FN static int _cffi_to_c_char32_t(PyObject *o) +{ + if (sizeof(_cffi_wchar_t) == 4) + return (int)_cffi_to_c_wchar_t(o); + else + return (int)_cffi_to_c_wchar3216_t(o); +} + +_CFFI_UNUSED_FN static PyObject *_cffi_from_c_char32_t(int x) +{ + if (sizeof(_cffi_wchar_t) == 4) + return _cffi_from_c_wchar_t((_cffi_wchar_t)x); + else + return _cffi_from_c_wchar3216_t(x); +} + + +/********** end CPython-specific section **********/ +#else +_CFFI_UNUSED_FN +static void (*_cffi_call_python_org)(struct _cffi_externpy_s *, char *); +# define _cffi_call_python _cffi_call_python_org +#endif + + +#define _cffi_array_len(array) (sizeof(array) / sizeof((array)[0])) + +#define _cffi_prim_int(size, sign) \ + ((size) == 1 ? ((sign) ? _CFFI_PRIM_INT8 : _CFFI_PRIM_UINT8) : \ + (size) == 2 ? ((sign) ? _CFFI_PRIM_INT16 : _CFFI_PRIM_UINT16) : \ + (size) == 4 ? ((sign) ? _CFFI_PRIM_INT32 : _CFFI_PRIM_UINT32) : \ + (size) == 8 ? ((sign) ? _CFFI_PRIM_INT64 : _CFFI_PRIM_UINT64) : \ + _CFFI__UNKNOWN_PRIM) + +#define _cffi_prim_float(size) \ + ((size) == sizeof(float) ? _CFFI_PRIM_FLOAT : \ + (size) == sizeof(double) ? _CFFI_PRIM_DOUBLE : \ + (size) == sizeof(long double) ? _CFFI__UNKNOWN_LONG_DOUBLE : \ + _CFFI__UNKNOWN_FLOAT_PRIM) + +#define _cffi_check_int(got, got_nonpos, expected) \ + ((got_nonpos) == (expected <= 0) && \ + (got) == (unsigned long long)expected) + +#ifdef MS_WIN32 +# define _cffi_stdcall __stdcall +#else +# define _cffi_stdcall /* nothing */ +#endif + +#ifdef __cplusplus +} +#endif + +/************************************************************/ + +/****************************************************************************** + * Code generated with sympy 1.1.1 * + * * + * See http://www.sympy.org/ for more information. * + * * + * This file is part of 'ekf' * + ******************************************************************************/ +void err_fun(double *nom_x, double *delta_x, double *out_3826149132230839283) { + out_3826149132230839283[0] = delta_x[0] + nom_x[0]; + out_3826149132230839283[1] = delta_x[1] + nom_x[1]; + out_3826149132230839283[2] = delta_x[2] + nom_x[2]; + out_3826149132230839283[3] = delta_x[3] + nom_x[3]; + out_3826149132230839283[4] = delta_x[4] + nom_x[4]; + out_3826149132230839283[5] = delta_x[5] + nom_x[5]; + out_3826149132230839283[6] = delta_x[6] + nom_x[6]; + out_3826149132230839283[7] = delta_x[7] + nom_x[7]; + out_3826149132230839283[8] = delta_x[8] + nom_x[8]; + out_3826149132230839283[9] = delta_x[9] + nom_x[9]; + out_3826149132230839283[10] = delta_x[10] + nom_x[10]; + out_3826149132230839283[11] = delta_x[11] + nom_x[11]; + out_3826149132230839283[12] = delta_x[12] + nom_x[12]; +} +void inv_err_fun(double *nom_x, double *true_x, double *out_8151328542980574746) { + out_8151328542980574746[0] = -nom_x[0] + true_x[0]; + out_8151328542980574746[1] = -nom_x[1] + true_x[1]; + out_8151328542980574746[2] = -nom_x[2] + true_x[2]; + out_8151328542980574746[3] = -nom_x[3] + true_x[3]; + out_8151328542980574746[4] = -nom_x[4] + true_x[4]; + out_8151328542980574746[5] = -nom_x[5] + true_x[5]; + out_8151328542980574746[6] = -nom_x[6] + true_x[6]; + out_8151328542980574746[7] = -nom_x[7] + true_x[7]; + out_8151328542980574746[8] = -nom_x[8] + true_x[8]; + out_8151328542980574746[9] = -nom_x[9] + true_x[9]; + out_8151328542980574746[10] = -nom_x[10] + true_x[10]; + out_8151328542980574746[11] = -nom_x[11] + true_x[11]; + out_8151328542980574746[12] = -nom_x[12] + true_x[12]; +} +void H_mod_fun(double *state, double *out_5655986027143292857) { + out_5655986027143292857[0] = 1.0; + out_5655986027143292857[1] = 0.0; + out_5655986027143292857[2] = 0.0; + out_5655986027143292857[3] = 0.0; + out_5655986027143292857[4] = 0.0; + out_5655986027143292857[5] = 0.0; + out_5655986027143292857[6] = 0.0; + out_5655986027143292857[7] = 0.0; + out_5655986027143292857[8] = 0.0; + out_5655986027143292857[9] = 0.0; + out_5655986027143292857[10] = 0.0; + out_5655986027143292857[11] = 0.0; + out_5655986027143292857[12] = 0.0; + out_5655986027143292857[13] = 0.0; + out_5655986027143292857[14] = 1.0; + out_5655986027143292857[15] = 0.0; + out_5655986027143292857[16] = 0.0; + out_5655986027143292857[17] = 0.0; + out_5655986027143292857[18] = 0.0; + out_5655986027143292857[19] = 0.0; + out_5655986027143292857[20] = 0.0; + out_5655986027143292857[21] = 0.0; + out_5655986027143292857[22] = 0.0; + out_5655986027143292857[23] = 0.0; + out_5655986027143292857[24] = 0.0; + out_5655986027143292857[25] = 0.0; + out_5655986027143292857[26] = 0.0; + out_5655986027143292857[27] = 0.0; + out_5655986027143292857[28] = 1.0; + out_5655986027143292857[29] = 0.0; + out_5655986027143292857[30] = 0.0; + out_5655986027143292857[31] = 0.0; + out_5655986027143292857[32] = 0.0; + out_5655986027143292857[33] = 0.0; + out_5655986027143292857[34] = 0.0; + out_5655986027143292857[35] = 0.0; + out_5655986027143292857[36] = 0.0; + out_5655986027143292857[37] = 0.0; + out_5655986027143292857[38] = 0.0; + out_5655986027143292857[39] = 0.0; + out_5655986027143292857[40] = 0.0; + out_5655986027143292857[41] = 0.0; + out_5655986027143292857[42] = 1.0; + out_5655986027143292857[43] = 0.0; + out_5655986027143292857[44] = 0.0; + out_5655986027143292857[45] = 0.0; + out_5655986027143292857[46] = 0.0; + out_5655986027143292857[47] = 0.0; + out_5655986027143292857[48] = 0.0; + out_5655986027143292857[49] = 0.0; + out_5655986027143292857[50] = 0.0; + out_5655986027143292857[51] = 0.0; + out_5655986027143292857[52] = 0.0; + out_5655986027143292857[53] = 0.0; + out_5655986027143292857[54] = 0.0; + out_5655986027143292857[55] = 0.0; + out_5655986027143292857[56] = 1.0; + out_5655986027143292857[57] = 0.0; + out_5655986027143292857[58] = 0.0; + out_5655986027143292857[59] = 0.0; + out_5655986027143292857[60] = 0.0; + out_5655986027143292857[61] = 0.0; + out_5655986027143292857[62] = 0.0; + out_5655986027143292857[63] = 0.0; + out_5655986027143292857[64] = 0.0; + out_5655986027143292857[65] = 0.0; + out_5655986027143292857[66] = 0.0; + out_5655986027143292857[67] = 0.0; + out_5655986027143292857[68] = 0.0; + out_5655986027143292857[69] = 0.0; + out_5655986027143292857[70] = 1.0; + out_5655986027143292857[71] = 0.0; + out_5655986027143292857[72] = 0.0; + out_5655986027143292857[73] = 0.0; + out_5655986027143292857[74] = 0.0; + out_5655986027143292857[75] = 0.0; + out_5655986027143292857[76] = 0.0; + out_5655986027143292857[77] = 0.0; + out_5655986027143292857[78] = 0.0; + out_5655986027143292857[79] = 0.0; + out_5655986027143292857[80] = 0.0; + out_5655986027143292857[81] = 0.0; + out_5655986027143292857[82] = 0.0; + out_5655986027143292857[83] = 0.0; + out_5655986027143292857[84] = 1.0; + out_5655986027143292857[85] = 0.0; + out_5655986027143292857[86] = 0.0; + out_5655986027143292857[87] = 0.0; + out_5655986027143292857[88] = 0.0; + out_5655986027143292857[89] = 0.0; + out_5655986027143292857[90] = 0.0; + out_5655986027143292857[91] = 0.0; + out_5655986027143292857[92] = 0.0; + out_5655986027143292857[93] = 0.0; + out_5655986027143292857[94] = 0.0; + out_5655986027143292857[95] = 0.0; + out_5655986027143292857[96] = 0.0; + out_5655986027143292857[97] = 0.0; + out_5655986027143292857[98] = 1.0; + out_5655986027143292857[99] = 0.0; + out_5655986027143292857[100] = 0.0; + out_5655986027143292857[101] = 0.0; + out_5655986027143292857[102] = 0.0; + out_5655986027143292857[103] = 0.0; + out_5655986027143292857[104] = 0.0; + out_5655986027143292857[105] = 0.0; + out_5655986027143292857[106] = 0.0; + out_5655986027143292857[107] = 0.0; + out_5655986027143292857[108] = 0.0; + out_5655986027143292857[109] = 0.0; + out_5655986027143292857[110] = 0.0; + out_5655986027143292857[111] = 0.0; + out_5655986027143292857[112] = 1.0; + out_5655986027143292857[113] = 0.0; + out_5655986027143292857[114] = 0.0; + out_5655986027143292857[115] = 0.0; + out_5655986027143292857[116] = 0.0; + out_5655986027143292857[117] = 0.0; + out_5655986027143292857[118] = 0.0; + out_5655986027143292857[119] = 0.0; + out_5655986027143292857[120] = 0.0; + out_5655986027143292857[121] = 0.0; + out_5655986027143292857[122] = 0.0; + out_5655986027143292857[123] = 0.0; + out_5655986027143292857[124] = 0.0; + out_5655986027143292857[125] = 0.0; + out_5655986027143292857[126] = 1.0; + out_5655986027143292857[127] = 0.0; + out_5655986027143292857[128] = 0.0; + out_5655986027143292857[129] = 0.0; + out_5655986027143292857[130] = 0.0; + out_5655986027143292857[131] = 0.0; + out_5655986027143292857[132] = 0.0; + out_5655986027143292857[133] = 0.0; + out_5655986027143292857[134] = 0.0; + out_5655986027143292857[135] = 0.0; + out_5655986027143292857[136] = 0.0; + out_5655986027143292857[137] = 0.0; + out_5655986027143292857[138] = 0.0; + out_5655986027143292857[139] = 0.0; + out_5655986027143292857[140] = 1.0; + out_5655986027143292857[141] = 0.0; + out_5655986027143292857[142] = 0.0; + out_5655986027143292857[143] = 0.0; + out_5655986027143292857[144] = 0.0; + out_5655986027143292857[145] = 0.0; + out_5655986027143292857[146] = 0.0; + out_5655986027143292857[147] = 0.0; + out_5655986027143292857[148] = 0.0; + out_5655986027143292857[149] = 0.0; + out_5655986027143292857[150] = 0.0; + out_5655986027143292857[151] = 0.0; + out_5655986027143292857[152] = 0.0; + out_5655986027143292857[153] = 0.0; + out_5655986027143292857[154] = 1.0; + out_5655986027143292857[155] = 0.0; + out_5655986027143292857[156] = 0.0; + out_5655986027143292857[157] = 0.0; + out_5655986027143292857[158] = 0.0; + out_5655986027143292857[159] = 0.0; + out_5655986027143292857[160] = 0.0; + out_5655986027143292857[161] = 0.0; + out_5655986027143292857[162] = 0.0; + out_5655986027143292857[163] = 0.0; + out_5655986027143292857[164] = 0.0; + out_5655986027143292857[165] = 0.0; + out_5655986027143292857[166] = 0.0; + out_5655986027143292857[167] = 0.0; + out_5655986027143292857[168] = 1.0; +} +void f_fun(double *state, double dt, double *out_2426912954331896736) { + out_2426912954331896736[0] = dt*state[10] + state[0]; + out_2426912954331896736[1] = dt*state[11] + state[1]; + out_2426912954331896736[2] = dt*state[12] + state[2]; + out_2426912954331896736[3] = state[3]; + out_2426912954331896736[4] = state[4]; + out_2426912954331896736[5] = state[5]; + out_2426912954331896736[6] = state[6]; + out_2426912954331896736[7] = state[7]; + out_2426912954331896736[8] = state[8]; + out_2426912954331896736[9] = state[9]; + out_2426912954331896736[10] = state[10]; + out_2426912954331896736[11] = state[11]; + out_2426912954331896736[12] = state[12]; +} +void F_fun(double *state, double dt, double *out_1828163745478812813) { + out_1828163745478812813[0] = 1; + out_1828163745478812813[1] = 0; + out_1828163745478812813[2] = 0; + out_1828163745478812813[3] = 0; + out_1828163745478812813[4] = 0; + out_1828163745478812813[5] = 0; + out_1828163745478812813[6] = 0; + out_1828163745478812813[7] = 0; + out_1828163745478812813[8] = 0; + out_1828163745478812813[9] = 0; + out_1828163745478812813[10] = dt; + out_1828163745478812813[11] = 0; + out_1828163745478812813[12] = 0; + out_1828163745478812813[13] = 0; + out_1828163745478812813[14] = 1; + out_1828163745478812813[15] = 0; + out_1828163745478812813[16] = 0; + out_1828163745478812813[17] = 0; + out_1828163745478812813[18] = 0; + out_1828163745478812813[19] = 0; + out_1828163745478812813[20] = 0; + out_1828163745478812813[21] = 0; + out_1828163745478812813[22] = 0; + out_1828163745478812813[23] = 0; + out_1828163745478812813[24] = dt; + out_1828163745478812813[25] = 0; + out_1828163745478812813[26] = 0; + out_1828163745478812813[27] = 0; + out_1828163745478812813[28] = 1; + out_1828163745478812813[29] = 0; + out_1828163745478812813[30] = 0; + out_1828163745478812813[31] = 0; + out_1828163745478812813[32] = 0; + out_1828163745478812813[33] = 0; + out_1828163745478812813[34] = 0; + out_1828163745478812813[35] = 0; + out_1828163745478812813[36] = 0; + out_1828163745478812813[37] = 0; + out_1828163745478812813[38] = dt; + out_1828163745478812813[39] = 0; + out_1828163745478812813[40] = 0; + out_1828163745478812813[41] = 0; + out_1828163745478812813[42] = 1; + out_1828163745478812813[43] = 0; + out_1828163745478812813[44] = 0; + out_1828163745478812813[45] = 0; + out_1828163745478812813[46] = 0; + out_1828163745478812813[47] = 0; + out_1828163745478812813[48] = 0; + out_1828163745478812813[49] = 0; + out_1828163745478812813[50] = 0; + out_1828163745478812813[51] = 0; + out_1828163745478812813[52] = 0; + out_1828163745478812813[53] = 0; + out_1828163745478812813[54] = 0; + out_1828163745478812813[55] = 0; + out_1828163745478812813[56] = 1; + out_1828163745478812813[57] = 0; + out_1828163745478812813[58] = 0; + out_1828163745478812813[59] = 0; + out_1828163745478812813[60] = 0; + out_1828163745478812813[61] = 0; + out_1828163745478812813[62] = 0; + out_1828163745478812813[63] = 0; + out_1828163745478812813[64] = 0; + out_1828163745478812813[65] = 0; + out_1828163745478812813[66] = 0; + out_1828163745478812813[67] = 0; + out_1828163745478812813[68] = 0; + out_1828163745478812813[69] = 0; + out_1828163745478812813[70] = 1; + out_1828163745478812813[71] = 0; + out_1828163745478812813[72] = 0; + out_1828163745478812813[73] = 0; + out_1828163745478812813[74] = 0; + out_1828163745478812813[75] = 0; + out_1828163745478812813[76] = 0; + out_1828163745478812813[77] = 0; + out_1828163745478812813[78] = 0; + out_1828163745478812813[79] = 0; + out_1828163745478812813[80] = 0; + out_1828163745478812813[81] = 0; + out_1828163745478812813[82] = 0; + out_1828163745478812813[83] = 0; + out_1828163745478812813[84] = 1; + out_1828163745478812813[85] = 0; + out_1828163745478812813[86] = 0; + out_1828163745478812813[87] = 0; + out_1828163745478812813[88] = 0; + out_1828163745478812813[89] = 0; + out_1828163745478812813[90] = 0; + out_1828163745478812813[91] = 0; + out_1828163745478812813[92] = 0; + out_1828163745478812813[93] = 0; + out_1828163745478812813[94] = 0; + out_1828163745478812813[95] = 0; + out_1828163745478812813[96] = 0; + out_1828163745478812813[97] = 0; + out_1828163745478812813[98] = 1; + out_1828163745478812813[99] = 0; + out_1828163745478812813[100] = 0; + out_1828163745478812813[101] = 0; + out_1828163745478812813[102] = 0; + out_1828163745478812813[103] = 0; + out_1828163745478812813[104] = 0; + out_1828163745478812813[105] = 0; + out_1828163745478812813[106] = 0; + out_1828163745478812813[107] = 0; + out_1828163745478812813[108] = 0; + out_1828163745478812813[109] = 0; + out_1828163745478812813[110] = 0; + out_1828163745478812813[111] = 0; + out_1828163745478812813[112] = 1; + out_1828163745478812813[113] = 0; + out_1828163745478812813[114] = 0; + out_1828163745478812813[115] = 0; + out_1828163745478812813[116] = 0; + out_1828163745478812813[117] = 0; + out_1828163745478812813[118] = 0; + out_1828163745478812813[119] = 0; + out_1828163745478812813[120] = 0; + out_1828163745478812813[121] = 0; + out_1828163745478812813[122] = 0; + out_1828163745478812813[123] = 0; + out_1828163745478812813[124] = 0; + out_1828163745478812813[125] = 0; + out_1828163745478812813[126] = 1; + out_1828163745478812813[127] = 0; + out_1828163745478812813[128] = 0; + out_1828163745478812813[129] = 0; + out_1828163745478812813[130] = 0; + out_1828163745478812813[131] = 0; + out_1828163745478812813[132] = 0; + out_1828163745478812813[133] = 0; + out_1828163745478812813[134] = 0; + out_1828163745478812813[135] = 0; + out_1828163745478812813[136] = 0; + out_1828163745478812813[137] = 0; + out_1828163745478812813[138] = 0; + out_1828163745478812813[139] = 0; + out_1828163745478812813[140] = 1; + out_1828163745478812813[141] = 0; + out_1828163745478812813[142] = 0; + out_1828163745478812813[143] = 0; + out_1828163745478812813[144] = 0; + out_1828163745478812813[145] = 0; + out_1828163745478812813[146] = 0; + out_1828163745478812813[147] = 0; + out_1828163745478812813[148] = 0; + out_1828163745478812813[149] = 0; + out_1828163745478812813[150] = 0; + out_1828163745478812813[151] = 0; + out_1828163745478812813[152] = 0; + out_1828163745478812813[153] = 0; + out_1828163745478812813[154] = 1; + out_1828163745478812813[155] = 0; + out_1828163745478812813[156] = 0; + out_1828163745478812813[157] = 0; + out_1828163745478812813[158] = 0; + out_1828163745478812813[159] = 0; + out_1828163745478812813[160] = 0; + out_1828163745478812813[161] = 0; + out_1828163745478812813[162] = 0; + out_1828163745478812813[163] = 0; + out_1828163745478812813[164] = 0; + out_1828163745478812813[165] = 0; + out_1828163745478812813[166] = 0; + out_1828163745478812813[167] = 0; + out_1828163745478812813[168] = 1; +} +void h_3(double *state, double *unused, double *out_4528959146572613286) { + out_4528959146572613286[0] = sqrt(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2))*state[9]; +} +void H_3(double *state, double *unused, double *out_4776213603435969308) { + out_4776213603435969308[0] = state[0]*state[9]/sqrt(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2)); + out_4776213603435969308[1] = state[1]*state[9]/sqrt(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2)); + out_4776213603435969308[2] = state[2]*state[9]/sqrt(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2)); + out_4776213603435969308[3] = 0; + out_4776213603435969308[4] = 0; + out_4776213603435969308[5] = 0; + out_4776213603435969308[6] = 0; + out_4776213603435969308[7] = 0; + out_4776213603435969308[8] = 0; + out_4776213603435969308[9] = sqrt(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2)); + out_4776213603435969308[10] = 0; + out_4776213603435969308[11] = 0; + out_4776213603435969308[12] = 0; +} +void h_4(double *state, double *unused, double *out_7897607812907042004) { + out_7897607812907042004[0] = state[3] + state[6]; + out_7897607812907042004[1] = state[4] + state[7]; + out_7897607812907042004[2] = state[5] + state[8]; +} +void H_4(double *state, double *unused, double *out_7822787049155785962) { + out_7822787049155785962[0] = 0; + out_7822787049155785962[1] = 0; + out_7822787049155785962[2] = 0; + out_7822787049155785962[3] = 1; + out_7822787049155785962[4] = 0; + out_7822787049155785962[5] = 0; + out_7822787049155785962[6] = 1; + out_7822787049155785962[7] = 0; + out_7822787049155785962[8] = 0; + out_7822787049155785962[9] = 0; + out_7822787049155785962[10] = 0; + out_7822787049155785962[11] = 0; + out_7822787049155785962[12] = 0; + out_7822787049155785962[13] = 0; + out_7822787049155785962[14] = 0; + out_7822787049155785962[15] = 0; + out_7822787049155785962[16] = 0; + out_7822787049155785962[17] = 1; + out_7822787049155785962[18] = 0; + out_7822787049155785962[19] = 0; + out_7822787049155785962[20] = 1; + out_7822787049155785962[21] = 0; + out_7822787049155785962[22] = 0; + out_7822787049155785962[23] = 0; + out_7822787049155785962[24] = 0; + out_7822787049155785962[25] = 0; + out_7822787049155785962[26] = 0; + out_7822787049155785962[27] = 0; + out_7822787049155785962[28] = 0; + out_7822787049155785962[29] = 0; + out_7822787049155785962[30] = 0; + out_7822787049155785962[31] = 1; + out_7822787049155785962[32] = 0; + out_7822787049155785962[33] = 0; + out_7822787049155785962[34] = 1; + out_7822787049155785962[35] = 0; + out_7822787049155785962[36] = 0; + out_7822787049155785962[37] = 0; + out_7822787049155785962[38] = 0; +} +void h_9(double *state, double *unused, double *out_5299392560341573238) { + out_5299392560341573238[0] = state[3]; + out_5299392560341573238[1] = state[4]; + out_5299392560341573238[2] = state[5]; +} +void H_9(double *state, double *unused, double *out_2816358492518891651) { + out_2816358492518891651[0] = 0; + out_2816358492518891651[1] = 0; + out_2816358492518891651[2] = 0; + out_2816358492518891651[3] = 1; + out_2816358492518891651[4] = 0; + out_2816358492518891651[5] = 0; + out_2816358492518891651[6] = 0; + out_2816358492518891651[7] = 0; + out_2816358492518891651[8] = 0; + out_2816358492518891651[9] = 0; + out_2816358492518891651[10] = 0; + out_2816358492518891651[11] = 0; + out_2816358492518891651[12] = 0; + out_2816358492518891651[13] = 0; + out_2816358492518891651[14] = 0; + out_2816358492518891651[15] = 0; + out_2816358492518891651[16] = 0; + out_2816358492518891651[17] = 1; + out_2816358492518891651[18] = 0; + out_2816358492518891651[19] = 0; + out_2816358492518891651[20] = 0; + out_2816358492518891651[21] = 0; + out_2816358492518891651[22] = 0; + out_2816358492518891651[23] = 0; + out_2816358492518891651[24] = 0; + out_2816358492518891651[25] = 0; + out_2816358492518891651[26] = 0; + out_2816358492518891651[27] = 0; + out_2816358492518891651[28] = 0; + out_2816358492518891651[29] = 0; + out_2816358492518891651[30] = 0; + out_2816358492518891651[31] = 1; + out_2816358492518891651[32] = 0; + out_2816358492518891651[33] = 0; + out_2816358492518891651[34] = 0; + out_2816358492518891651[35] = 0; + out_2816358492518891651[36] = 0; + out_2816358492518891651[37] = 0; + out_2816358492518891651[38] = 0; +} +void h_13(double *state, double *unused, double *out_4549566257314944719) { + out_4549566257314944719[0] = state[0]; + out_4549566257314944719[1] = state[1]; + out_4549566257314944719[2] = state[2]; +} +void H_13(double *state, double *unused, double *out_3504968933202572761) { + out_3504968933202572761[0] = 1; + out_3504968933202572761[1] = 0; + out_3504968933202572761[2] = 0; + out_3504968933202572761[3] = 0; + out_3504968933202572761[4] = 0; + out_3504968933202572761[5] = 0; + out_3504968933202572761[6] = 0; + out_3504968933202572761[7] = 0; + out_3504968933202572761[8] = 0; + out_3504968933202572761[9] = 0; + out_3504968933202572761[10] = 0; + out_3504968933202572761[11] = 0; + out_3504968933202572761[12] = 0; + out_3504968933202572761[13] = 0; + out_3504968933202572761[14] = 1; + out_3504968933202572761[15] = 0; + out_3504968933202572761[16] = 0; + out_3504968933202572761[17] = 0; + out_3504968933202572761[18] = 0; + out_3504968933202572761[19] = 0; + out_3504968933202572761[20] = 0; + out_3504968933202572761[21] = 0; + out_3504968933202572761[22] = 0; + out_3504968933202572761[23] = 0; + out_3504968933202572761[24] = 0; + out_3504968933202572761[25] = 0; + out_3504968933202572761[26] = 0; + out_3504968933202572761[27] = 0; + out_3504968933202572761[28] = 1; + out_3504968933202572761[29] = 0; + out_3504968933202572761[30] = 0; + out_3504968933202572761[31] = 0; + out_3504968933202572761[32] = 0; + out_3504968933202572761[33] = 0; + out_3504968933202572761[34] = 0; + out_3504968933202572761[35] = 0; + out_3504968933202572761[36] = 0; + out_3504968933202572761[37] = 0; + out_3504968933202572761[38] = 0; +} +void h_14(double *state, double *unused, double *out_5299392560341573238) { + out_5299392560341573238[0] = state[3]; + out_5299392560341573238[1] = state[4]; + out_5299392560341573238[2] = state[5]; +} +void H_14(double *state, double *unused, double *out_2816358492518891651) { + out_2816358492518891651[0] = 0; + out_2816358492518891651[1] = 0; + out_2816358492518891651[2] = 0; + out_2816358492518891651[3] = 1; + out_2816358492518891651[4] = 0; + out_2816358492518891651[5] = 0; + out_2816358492518891651[6] = 0; + out_2816358492518891651[7] = 0; + out_2816358492518891651[8] = 0; + out_2816358492518891651[9] = 0; + out_2816358492518891651[10] = 0; + out_2816358492518891651[11] = 0; + out_2816358492518891651[12] = 0; + out_2816358492518891651[13] = 0; + out_2816358492518891651[14] = 0; + out_2816358492518891651[15] = 0; + out_2816358492518891651[16] = 0; + out_2816358492518891651[17] = 1; + out_2816358492518891651[18] = 0; + out_2816358492518891651[19] = 0; + out_2816358492518891651[20] = 0; + out_2816358492518891651[21] = 0; + out_2816358492518891651[22] = 0; + out_2816358492518891651[23] = 0; + out_2816358492518891651[24] = 0; + out_2816358492518891651[25] = 0; + out_2816358492518891651[26] = 0; + out_2816358492518891651[27] = 0; + out_2816358492518891651[28] = 0; + out_2816358492518891651[29] = 0; + out_2816358492518891651[30] = 0; + out_2816358492518891651[31] = 1; + out_2816358492518891651[32] = 0; + out_2816358492518891651[33] = 0; + out_2816358492518891651[34] = 0; + out_2816358492518891651[35] = 0; + out_2816358492518891651[36] = 0; + out_2816358492518891651[37] = 0; + out_2816358492518891651[38] = 0; +} +#define DIM 13 +#define EDIM 13 +#define MEDIM 13 +typedef void (*Hfun)(double *, double *, double *); + +void predict(double *x, double *P, double *Q, double dt); +const static double MAHA_THRESH_3 = 3.841459; +void update_3(double *, double *, double *, double *, double *); +const static double MAHA_THRESH_4 = 7.814728; +void update_4(double *, double *, double *, double *, double *); +const static double MAHA_THRESH_9 = 7.814728; +void update_9(double *, double *, double *, double *, double *); +const static double MAHA_THRESH_13 = 7.814728; +void update_13(double *, double *, double *, double *, double *); +const static double MAHA_THRESH_14 = 7.814728; +void update_14(double *, double *, double *, double *, double *); +#include +#include + +typedef Eigen::Matrix DDM; +typedef Eigen::Matrix EEM; +typedef Eigen::Matrix DEM; + +void predict(double *in_x, double *in_P, double *in_Q, double dt) { + typedef Eigen::Matrix RRM; + + double nx[DIM] = {0}; + double in_F[EDIM*EDIM] = {0}; + + // functions from sympy + f_fun(in_x, dt, nx); + F_fun(in_x, dt, in_F); + + + EEM F(in_F); + EEM P(in_P); + EEM Q(in_Q); + + RRM F_main = F.topLeftCorner(MEDIM, MEDIM); + P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose(); + P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM); + P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose(); + + P = P + dt*Q; + + // copy out state + memcpy(in_x, nx, DIM * sizeof(double)); + memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double)); +} + +// note: extra_args dim only correct when null space projecting +// otherwise 1 +template +void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) { + typedef Eigen::Matrix ZZM; + typedef Eigen::Matrix ZDM; + typedef Eigen::Matrix ZEM; + typedef Eigen::Matrix XEM; + typedef Eigen::Matrix EZM; + typedef Eigen::Matrix X1M; + typedef Eigen::Matrix XXM; + + double in_hx[ZDIM] = {0}; + double in_H[ZDIM * DIM] = {0}; + double in_H_mod[EDIM * DIM] = {0}; + double delta_x[EDIM] = {0}; + double x_new[DIM] = {0}; + + + // state x, P + Eigen::Matrix z(in_z); + EEM P(in_P); + ZZM pre_R(in_R); + + // functions from sympy + h_fun(in_x, in_ea, in_hx); + H_fun(in_x, in_ea, in_H); + ZDM pre_H(in_H); + + // get y (y = z - hx) + Eigen::Matrix pre_y(in_hx); pre_y = z - pre_y; + X1M y; XXM H; XXM R; + if (Hea_fun){ + typedef Eigen::Matrix ZAM; + double in_Hea[ZDIM * EADIM] = {0}; + Hea_fun(in_x, in_ea, in_Hea); + ZAM Hea(in_Hea); + XXM A = Hea.transpose().fullPivLu().kernel(); + + + y = A.transpose() * pre_y; + H = A.transpose() * pre_H; + R = A.transpose() * pre_R * A; + } else { + y = pre_y; + H = pre_H; + R = pre_R; + } + // get modified H + H_mod_fun(in_x, in_H_mod); + DEM H_mod(in_H_mod); + XEM H_err = H * H_mod; + + // Do mahalobis distance test + if (MAHA_TEST){ + XXM a = (H_err * P * H_err.transpose() + R).inverse(); + double maha_dist = y.transpose() * a * y; + if (maha_dist > MAHA_THRESHOLD){ + R = 1.0e16 * R; + } + } + + // Outlier resilient weighting + double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum()); + + // kalman gains and I_KH + XXM S = ((H_err * P) * H_err.transpose()) + R/weight; + XEM KT = S.fullPivLu().solve(H_err * P.transpose()); + //EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE? + //EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose(); + //std::cout << "Here is the matrix rot:\n" << K << std::endl; + EEM I_KH = Eigen::Matrix::Identity() - (KT.transpose() * H_err); + + // update state by injecting dx + Eigen::Matrix dx(delta_x); + dx = (KT.transpose() * y); + memcpy(delta_x, dx.data(), EDIM * sizeof(double)); + err_fun(in_x, delta_x, x_new); + Eigen::Matrix x(x_new); + + // update cov + P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT); + + // copy out state + memcpy(in_x, x.data(), DIM * sizeof(double)); + memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double)); + memcpy(in_z, y.data(), y.rows() * sizeof(double)); +} + + + + + void update_3(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { + update<1,3,0>(in_x, in_P, h_3, H_3, NULL, in_z, in_R, in_ea, MAHA_THRESH_3); + } + + void update_4(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { + update<3,3,0>(in_x, in_P, h_4, H_4, NULL, in_z, in_R, in_ea, MAHA_THRESH_4); + } + + void update_9(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { + update<3,3,0>(in_x, in_P, h_9, H_9, NULL, in_z, in_R, in_ea, MAHA_THRESH_9); + } + + void update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { + update<3,3,0>(in_x, in_P, h_13, H_13, NULL, in_z, in_R, in_ea, MAHA_THRESH_13); + } + + void update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { + update<3,3,0>(in_x, in_P, h_14, H_14, NULL, in_z, in_R, in_ea, MAHA_THRESH_14); + } + + +/************************************************************/ + +static void *_cffi_types[] = { +/* 0 */ _CFFI_OP(_CFFI_OP_FUNCTION, 28), // void()(double *, double *) +/* 1 */ _CFFI_OP(_CFFI_OP_POINTER, 20), // double * +/* 2 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 3 */ _CFFI_OP(_CFFI_OP_FUNCTION_END, 0), +/* 4 */ _CFFI_OP(_CFFI_OP_FUNCTION, 28), // void()(double *, double *, double *) +/* 5 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 6 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 7 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 8 */ _CFFI_OP(_CFFI_OP_FUNCTION_END, 0), +/* 9 */ _CFFI_OP(_CFFI_OP_FUNCTION, 28), // void()(double *, double *, double *, double *, double *) +/* 10 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 11 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 12 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 13 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 14 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 15 */ _CFFI_OP(_CFFI_OP_FUNCTION_END, 0), +/* 16 */ _CFFI_OP(_CFFI_OP_FUNCTION, 28), // void()(double *, double *, double *, double) +/* 17 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 18 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 19 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 20 */ _CFFI_OP(_CFFI_OP_PRIMITIVE, 14), // double +/* 21 */ _CFFI_OP(_CFFI_OP_FUNCTION_END, 0), +/* 22 */ _CFFI_OP(_CFFI_OP_FUNCTION, 28), // void()(double *, double, double *) +/* 23 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 24 */ _CFFI_OP(_CFFI_OP_PRIMITIVE, 14), +/* 25 */ _CFFI_OP(_CFFI_OP_NOOP, 1), +/* 26 */ _CFFI_OP(_CFFI_OP_FUNCTION_END, 0), +/* 27 */ _CFFI_OP(_CFFI_OP_POINTER, 4), // void(*)(double *, double *, double *) +/* 28 */ _CFFI_OP(_CFFI_OP_PRIMITIVE, 0), // void +}; + +static void _cffi_const_MAHA_THRESH_13(char *o) +{ + *(double *)o = MAHA_THRESH_13; +} + +static void _cffi_const_MAHA_THRESH_14(char *o) +{ + *(double *)o = MAHA_THRESH_14; +} + +static void _cffi_const_MAHA_THRESH_3(char *o) +{ + *(double *)o = MAHA_THRESH_3; +} + +static void _cffi_const_MAHA_THRESH_4(char *o) +{ + *(double *)o = MAHA_THRESH_4; +} + +static void _cffi_const_MAHA_THRESH_9(char *o) +{ + *(double *)o = MAHA_THRESH_9; +} + +static void _cffi_d_F_fun(double * x0, double x1, double * x2) +{ + F_fun(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_F_fun(PyObject *self, PyObject *args) +{ + double * x0; + double x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "F_fun", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + x1 = (double)_cffi_to_c_double(arg1); + if (x1 == (double)-1 && PyErr_Occurred()) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { F_fun(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_F_fun _cffi_d_F_fun +#endif + +static void _cffi_d_H_13(double * x0, double * x1, double * x2) +{ + H_13(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_H_13(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "H_13", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { H_13(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_H_13 _cffi_d_H_13 +#endif + +static void _cffi_d_H_14(double * x0, double * x1, double * x2) +{ + H_14(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_H_14(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "H_14", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { H_14(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_H_14 _cffi_d_H_14 +#endif + +static void _cffi_d_H_3(double * x0, double * x1, double * x2) +{ + H_3(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_H_3(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "H_3", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { H_3(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_H_3 _cffi_d_H_3 +#endif + +static void _cffi_d_H_4(double * x0, double * x1, double * x2) +{ + H_4(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_H_4(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "H_4", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { H_4(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_H_4 _cffi_d_H_4 +#endif + +static void _cffi_d_H_9(double * x0, double * x1, double * x2) +{ + H_9(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_H_9(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "H_9", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { H_9(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_H_9 _cffi_d_H_9 +#endif + +static void _cffi_d_H_mod_fun(double * x0, double * x1) +{ + H_mod_fun(x0, x1); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_H_mod_fun(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + + if (!PyArg_UnpackTuple(args, "H_mod_fun", 2, 2, &arg0, &arg1)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { H_mod_fun(x0, x1); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_H_mod_fun _cffi_d_H_mod_fun +#endif + +static void _cffi_d_err_fun(double * x0, double * x1, double * x2) +{ + err_fun(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_err_fun(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "err_fun", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { err_fun(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_err_fun _cffi_d_err_fun +#endif + +static void _cffi_d_f_fun(double * x0, double x1, double * x2) +{ + f_fun(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_f_fun(PyObject *self, PyObject *args) +{ + double * x0; + double x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "f_fun", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + x1 = (double)_cffi_to_c_double(arg1); + if (x1 == (double)-1 && PyErr_Occurred()) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { f_fun(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_f_fun _cffi_d_f_fun +#endif + +static void _cffi_d_h_13(double * x0, double * x1, double * x2) +{ + h_13(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_h_13(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "h_13", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { h_13(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_h_13 _cffi_d_h_13 +#endif + +static void _cffi_d_h_14(double * x0, double * x1, double * x2) +{ + h_14(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_h_14(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "h_14", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { h_14(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_h_14 _cffi_d_h_14 +#endif + +static void _cffi_d_h_3(double * x0, double * x1, double * x2) +{ + h_3(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_h_3(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "h_3", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { h_3(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_h_3 _cffi_d_h_3 +#endif + +static void _cffi_d_h_4(double * x0, double * x1, double * x2) +{ + h_4(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_h_4(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "h_4", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { h_4(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_h_4 _cffi_d_h_4 +#endif + +static void _cffi_d_h_9(double * x0, double * x1, double * x2) +{ + h_9(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_h_9(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "h_9", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { h_9(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_h_9 _cffi_d_h_9 +#endif + +static void _cffi_d_inv_err_fun(double * x0, double * x1, double * x2) +{ + inv_err_fun(x0, x1, x2); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_inv_err_fun(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + + if (!PyArg_UnpackTuple(args, "inv_err_fun", 3, 3, &arg0, &arg1, &arg2)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { inv_err_fun(x0, x1, x2); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_inv_err_fun _cffi_d_inv_err_fun +#endif + +static void _cffi_d_predict(double * x0, double * x1, double * x2, double x3) +{ + predict(x0, x1, x2, x3); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_predict(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + double x3; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + PyObject *arg3; + + if (!PyArg_UnpackTuple(args, "predict", 4, 4, &arg0, &arg1, &arg2, &arg3)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + x3 = (double)_cffi_to_c_double(arg3); + if (x3 == (double)-1 && PyErr_Occurred()) + return NULL; + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { predict(x0, x1, x2, x3); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_predict _cffi_d_predict +#endif + +static void _cffi_d_update_13(double * x0, double * x1, double * x2, double * x3, double * x4) +{ + update_13(x0, x1, x2, x3, x4); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_update_13(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + double * x3; + double * x4; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + PyObject *arg3; + PyObject *arg4; + + if (!PyArg_UnpackTuple(args, "update_13", 5, 5, &arg0, &arg1, &arg2, &arg3, &arg4)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg3, (char **)&x3); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x3 = (double *)alloca((size_t)datasize); + memset((void *)x3, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x3, _cffi_type(1), arg3) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg4, (char **)&x4); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x4 = (double *)alloca((size_t)datasize); + memset((void *)x4, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x4, _cffi_type(1), arg4) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { update_13(x0, x1, x2, x3, x4); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_update_13 _cffi_d_update_13 +#endif + +static void _cffi_d_update_14(double * x0, double * x1, double * x2, double * x3, double * x4) +{ + update_14(x0, x1, x2, x3, x4); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_update_14(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + double * x3; + double * x4; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + PyObject *arg3; + PyObject *arg4; + + if (!PyArg_UnpackTuple(args, "update_14", 5, 5, &arg0, &arg1, &arg2, &arg3, &arg4)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg3, (char **)&x3); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x3 = (double *)alloca((size_t)datasize); + memset((void *)x3, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x3, _cffi_type(1), arg3) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg4, (char **)&x4); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x4 = (double *)alloca((size_t)datasize); + memset((void *)x4, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x4, _cffi_type(1), arg4) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { update_14(x0, x1, x2, x3, x4); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_update_14 _cffi_d_update_14 +#endif + +static void _cffi_d_update_3(double * x0, double * x1, double * x2, double * x3, double * x4) +{ + update_3(x0, x1, x2, x3, x4); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_update_3(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + double * x3; + double * x4; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + PyObject *arg3; + PyObject *arg4; + + if (!PyArg_UnpackTuple(args, "update_3", 5, 5, &arg0, &arg1, &arg2, &arg3, &arg4)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg3, (char **)&x3); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x3 = (double *)alloca((size_t)datasize); + memset((void *)x3, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x3, _cffi_type(1), arg3) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg4, (char **)&x4); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x4 = (double *)alloca((size_t)datasize); + memset((void *)x4, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x4, _cffi_type(1), arg4) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { update_3(x0, x1, x2, x3, x4); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_update_3 _cffi_d_update_3 +#endif + +static void _cffi_d_update_4(double * x0, double * x1, double * x2, double * x3, double * x4) +{ + update_4(x0, x1, x2, x3, x4); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_update_4(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + double * x3; + double * x4; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + PyObject *arg3; + PyObject *arg4; + + if (!PyArg_UnpackTuple(args, "update_4", 5, 5, &arg0, &arg1, &arg2, &arg3, &arg4)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg3, (char **)&x3); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x3 = (double *)alloca((size_t)datasize); + memset((void *)x3, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x3, _cffi_type(1), arg3) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg4, (char **)&x4); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x4 = (double *)alloca((size_t)datasize); + memset((void *)x4, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x4, _cffi_type(1), arg4) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { update_4(x0, x1, x2, x3, x4); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_update_4 _cffi_d_update_4 +#endif + +static void _cffi_d_update_9(double * x0, double * x1, double * x2, double * x3, double * x4) +{ + update_9(x0, x1, x2, x3, x4); +} +#ifndef PYPY_VERSION +static PyObject * +_cffi_f_update_9(PyObject *self, PyObject *args) +{ + double * x0; + double * x1; + double * x2; + double * x3; + double * x4; + Py_ssize_t datasize; + PyObject *arg0; + PyObject *arg1; + PyObject *arg2; + PyObject *arg3; + PyObject *arg4; + + if (!PyArg_UnpackTuple(args, "update_9", 5, 5, &arg0, &arg1, &arg2, &arg3, &arg4)) + return NULL; + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg0, (char **)&x0); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x0 = (double *)alloca((size_t)datasize); + memset((void *)x0, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x0, _cffi_type(1), arg0) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg1, (char **)&x1); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x1 = (double *)alloca((size_t)datasize); + memset((void *)x1, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x1, _cffi_type(1), arg1) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg2, (char **)&x2); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x2 = (double *)alloca((size_t)datasize); + memset((void *)x2, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x2, _cffi_type(1), arg2) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg3, (char **)&x3); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x3 = (double *)alloca((size_t)datasize); + memset((void *)x3, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x3, _cffi_type(1), arg3) < 0) + return NULL; + } + + datasize = _cffi_prepare_pointer_call_argument( + _cffi_type(1), arg4, (char **)&x4); + if (datasize != 0) { + if (datasize < 0) + return NULL; + x4 = (double *)alloca((size_t)datasize); + memset((void *)x4, 0, (size_t)datasize); + if (_cffi_convert_array_from_object((char *)x4, _cffi_type(1), arg4) < 0) + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + _cffi_restore_errno(); + { update_9(x0, x1, x2, x3, x4); } + _cffi_save_errno(); + Py_END_ALLOW_THREADS + + (void)self; /* unused */ + Py_INCREF(Py_None); + return Py_None; +} +#else +# define _cffi_f_update_9 _cffi_d_update_9 +#endif + +static int _cffi_const_DIM(unsigned long long *o) +{ + int n = (DIM) <= 0; + *o = (unsigned long long)((DIM) | 0); /* check that DIM is an integer */ + if (!_cffi_check_int(*o, n, 13U)) + n |= 2; + return n; +} + +static int _cffi_const_EDIM(unsigned long long *o) +{ + int n = (EDIM) <= 0; + *o = (unsigned long long)((EDIM) | 0); /* check that EDIM is an integer */ + if (!_cffi_check_int(*o, n, 13U)) + n |= 2; + return n; +} + +static int _cffi_const_MEDIM(unsigned long long *o) +{ + int n = (MEDIM) <= 0; + *o = (unsigned long long)((MEDIM) | 0); /* check that MEDIM is an integer */ + if (!_cffi_check_int(*o, n, 13U)) + n |= 2; + return n; +} + +static const struct _cffi_global_s _cffi_globals[] = { + { "DIM", (void *)_cffi_const_DIM, _CFFI_OP(_CFFI_OP_CONSTANT_INT, -1), (void *)0 }, + { "EDIM", (void *)_cffi_const_EDIM, _CFFI_OP(_CFFI_OP_CONSTANT_INT, -1), (void *)0 }, + { "F_fun", (void *)_cffi_f_F_fun, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 22), (void *)_cffi_d_F_fun }, + { "H_13", (void *)_cffi_f_H_13, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_H_13 }, + { "H_14", (void *)_cffi_f_H_14, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_H_14 }, + { "H_3", (void *)_cffi_f_H_3, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_H_3 }, + { "H_4", (void *)_cffi_f_H_4, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_H_4 }, + { "H_9", (void *)_cffi_f_H_9, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_H_9 }, + { "H_mod_fun", (void *)_cffi_f_H_mod_fun, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 0), (void *)_cffi_d_H_mod_fun }, + { "MAHA_THRESH_13", (void *)_cffi_const_MAHA_THRESH_13, _CFFI_OP(_CFFI_OP_CONSTANT, 20), (void *)0 }, + { "MAHA_THRESH_14", (void *)_cffi_const_MAHA_THRESH_14, _CFFI_OP(_CFFI_OP_CONSTANT, 20), (void *)0 }, + { "MAHA_THRESH_3", (void *)_cffi_const_MAHA_THRESH_3, _CFFI_OP(_CFFI_OP_CONSTANT, 20), (void *)0 }, + { "MAHA_THRESH_4", (void *)_cffi_const_MAHA_THRESH_4, _CFFI_OP(_CFFI_OP_CONSTANT, 20), (void *)0 }, + { "MAHA_THRESH_9", (void *)_cffi_const_MAHA_THRESH_9, _CFFI_OP(_CFFI_OP_CONSTANT, 20), (void *)0 }, + { "MEDIM", (void *)_cffi_const_MEDIM, _CFFI_OP(_CFFI_OP_CONSTANT_INT, -1), (void *)0 }, + { "err_fun", (void *)_cffi_f_err_fun, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_err_fun }, + { "f_fun", (void *)_cffi_f_f_fun, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 22), (void *)_cffi_d_f_fun }, + { "h_13", (void *)_cffi_f_h_13, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_h_13 }, + { "h_14", (void *)_cffi_f_h_14, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_h_14 }, + { "h_3", (void *)_cffi_f_h_3, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_h_3 }, + { "h_4", (void *)_cffi_f_h_4, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_h_4 }, + { "h_9", (void *)_cffi_f_h_9, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_h_9 }, + { "inv_err_fun", (void *)_cffi_f_inv_err_fun, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 4), (void *)_cffi_d_inv_err_fun }, + { "predict", (void *)_cffi_f_predict, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 16), (void *)_cffi_d_predict }, + { "update_13", (void *)_cffi_f_update_13, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 9), (void *)_cffi_d_update_13 }, + { "update_14", (void *)_cffi_f_update_14, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 9), (void *)_cffi_d_update_14 }, + { "update_3", (void *)_cffi_f_update_3, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 9), (void *)_cffi_d_update_3 }, + { "update_4", (void *)_cffi_f_update_4, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 9), (void *)_cffi_d_update_4 }, + { "update_9", (void *)_cffi_f_update_9, _CFFI_OP(_CFFI_OP_CPYTHON_BLTN_V, 9), (void *)_cffi_d_update_9 }, +}; + +static const struct _cffi_typename_s _cffi_typenames[] = { + { "Hfun", 27 }, +}; + +static const struct _cffi_type_context_s _cffi_type_context = { + _cffi_types, + _cffi_globals, + NULL, /* no fields */ + NULL, /* no struct_unions */ + NULL, /* no enums */ + _cffi_typenames, + 29, /* num_globals */ + 0, /* num_struct_unions */ + 0, /* num_enums */ + 1, /* num_typenames */ + NULL, /* no includes */ + 29, /* num_types */ + 0, /* flags */ +}; + +#ifdef __GNUC__ +# pragma GCC visibility push(default) /* for -fvisibility= */ +#endif + +#ifdef PYPY_VERSION +PyMODINIT_FUNC +_cffi_pypyinit_loc_local(const void *p[]) +{ + p[0] = (const void *)0x2601; + p[1] = &_cffi_type_context; +#if PY_MAJOR_VERSION >= 3 + return NULL; +#endif +} +# ifdef _MSC_VER + PyMODINIT_FUNC +# if PY_MAJOR_VERSION >= 3 + PyInit_loc_local(void) { return NULL; } +# else + initloc_local(void) { } +# endif +# endif +#elif PY_MAJOR_VERSION >= 3 +PyMODINIT_FUNC +PyInit_loc_local(void) +{ + return _cffi_init("loc_local", 0x2601, &_cffi_type_context); +} +#else +PyMODINIT_FUNC +initloc_local(void) +{ + _cffi_init("loc_local", 0x2601, &_cffi_type_context); +} +#endif + +#ifdef __GNUC__ +# pragma GCC visibility pop +#endif diff --git a/selfdrive/locationd/locationd_local.py b/selfdrive/locationd/locationd_local.py index 0b1af2f4c15e35..95d05e183a16de 100755 --- a/selfdrive/locationd/locationd_local.py +++ b/selfdrive/locationd/locationd_local.py @@ -1,274 +1,287 @@ -#!/usr/bin/env python -import os -import zmq -import math -import json - -os.environ["OMP_NUM_THREADS"] = "1" -import numpy as np -from bisect import bisect_right - -from cereal import car -from common.params import Params -from common.numpy_fast import clip -import selfdrive.messaging as messaging -from selfdrive.swaglog import cloudlog -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.services import service_list -from selfdrive.locationd.kalman.loc_local_kf import LocLocalKalman -from selfdrive.locationd.kalman.kalman_helpers import ObservationKind - -DEBUG = False -kf = LocLocalKalman() # Make sure that model is generated on import time - -MAX_ANGLE_OFFSET = math.radians(10.) -MAX_ANGLE_OFFSET_TH = math.radians(9.) -MIN_STIFFNESS = 0.5 -MAX_STIFFNESS = 2.0 -MIN_SR = 0.5 -MAX_SR = 2.0 -MIN_SR_TH = 0.55 -MAX_SR_TH = 1.9 - -LEARNING_RATE = 3 - -class Localizer(object): - def __init__(self, disabled_logs=None, dog=None): - self.kf = LocLocalKalman() - self.reset_kalman() - - self.max_age = .2 # seconds - self.calibration_valid = False - - if disabled_logs is None: - self.disabled_logs = list() - else: - self.disabled_logs = disabled_logs - - def reset_kalman(self): - self.filter_time = None - self.observation_buffer = [] - self.converter = None - self.speed_counter = 0 - self.sensor_counter = 0 - - def liveLocationMsg(self, time): - fix = messaging.log.KalmanOdometry.new_message() - - predicted_state = self.kf.x - fix.trans = [float(predicted_state[0]), float(predicted_state[1]), float(predicted_state[2])] - fix.rot = [float(predicted_state[3]), float(predicted_state[4]), float(predicted_state[5])] - - return fix - - def update_kalman(self, time, kind, meas): - idx = bisect_right([x[0] for x in self.observation_buffer], time) - self.observation_buffer.insert(idx, (time, kind, meas)) - while self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age: - self.kf.predict_and_observe(*self.observation_buffer.pop(0)) - - def handle_cam_odo(self, log, current_time): - self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, np.concatenate([log.cameraOdometry.rot, - log.cameraOdometry.rotStd])) - self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans, - log.cameraOdometry.transStd])) - - def handle_car_state(self, log, current_time): - self.speed_counter += 1 - if self.speed_counter % 5 == 0: - self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.carState.vEgo])) - - def handle_sensors(self, log, current_time): - for sensor_reading in log.sensorEvents: - # TODO does not yet account for double sensor readings in the log - if sensor_reading.type == 4: - self.sensor_counter += 1 - if self.sensor_counter % LEARNING_RATE == 0: - self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-sensor_reading.gyro.v[2], -sensor_reading.gyro.v[1], -sensor_reading.gyro.v[0]]) - - def handle_log(self, log): - current_time = 1e-9 * log.logMonoTime - typ = log.which - if typ in self.disabled_logs: - return - if typ == "sensorEvents": - self.handle_sensors(log, current_time) - elif typ == "carState": - self.handle_car_state(log, current_time) - elif typ == "cameraOdometry": - self.handle_cam_odo(log, current_time) - - -class ParamsLearner(object): - def __init__(self, VM, angle_offset=0., stiffness_factor=1.0, steer_ratio=None, learning_rate=1.0): - self.VM = VM - - self.ao = math.radians(angle_offset) - self.slow_ao = math.radians(angle_offset) - self.x = stiffness_factor - self.sR = VM.sR if steer_ratio is None else steer_ratio - self.MIN_SR = MIN_SR * self.VM.sR - self.MAX_SR = MAX_SR * self.VM.sR - self.MIN_SR_TH = MIN_SR_TH * self.VM.sR - self.MAX_SR_TH = MAX_SR_TH * self.VM.sR - - self.alpha1 = 0.01 * learning_rate - self.alpha2 = 0.00025 * learning_rate - self.alpha3 = 0.1 * learning_rate - self.alpha4 = 1.0 * learning_rate - - def get_values(self): - return { - 'angleOffsetAverage': math.degrees(self.slow_ao), - 'stiffnessFactor': self.x, - 'steerRatio': self.sR, - } - - def update(self, psi, u, sa): - cF0 = self.VM.cF - cR0 = self.VM.cR - aR = self.VM.aR - aF = self.VM.aF - l = self.VM.l - m = self.VM.m - - x = self.x - ao = self.ao - sR = self.sR - - # Gradient descent: learn angle offset, tire stiffness and steer ratio. - if u > 10.0 and abs(math.degrees(sa)) < 15.: - self.ao -= self.alpha1 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2) - - ao = self.slow_ao - self.slow_ao -= self.alpha2 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2) - - self.x -= self.alpha3 * -2.0*cF0*cR0*l*m*u**3*(ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**3) - - self.sR -= self.alpha4 * -2.0*cF0*cR0*l*u*x*(ao - sa)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**3*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2) - - if DEBUG: - # s1 = "Measured yaw rate % .6f" % psi - # ao = 0. - # s2 = "Uncompensated yaw % .6f" % (1.0*u*(-ao + sa)/(l*sR*(1 - m*u**2*(aF*cF0*x - aR*cR0*x)/(cF0*cR0*l**2*x**2)))) - # instant_ao = aF*m*psi*sR*u/(cR0*l*x) - aR*m*psi*sR*u/(cF0*l*x) - l*psi*sR/u + sa - s4 = "Instant AO: % .2f Avg. AO % .2f" % (math.degrees(self.ao), math.degrees(self.slow_ao)) - s5 = "Stiffnes: % .3f x" % self.x - print s4, s5 - - - self.ao = clip(self.ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET) - self.slow_ao = clip(self.slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET) - self.x = clip(self.x, MIN_STIFFNESS, MAX_STIFFNESS) - self.sR = clip(self.sR, self.MIN_SR, self.MAX_SR) - - # don't check stiffness for validity, as it can change quickly if sR is off - valid = abs(self.slow_ao) < MAX_ANGLE_OFFSET_TH and \ - self.sR > self.MIN_SR_TH and self.sR < self.MAX_SR_TH - - return valid - - -def locationd_thread(gctx, addr, disabled_logs): - ctx = zmq.Context() - poller = zmq.Poller() - - car_state_socket = messaging.sub_sock(ctx, service_list['carState'].port, poller, addr=addr, conflate=True) - sensor_events_socket = messaging.sub_sock(ctx, service_list['sensorEvents'].port, poller, addr=addr, conflate=True) - camera_odometry_socket = messaging.sub_sock(ctx, service_list['cameraOdometry'].port, poller, addr=addr, conflate=True) - - kalman_odometry_socket = messaging.pub_sock(ctx, service_list['kalmanOdometry'].port) - live_parameters_socket = messaging.pub_sock(ctx, service_list['liveParameters'].port) - - params_reader = Params() - cloudlog.info("Parameter learner is waiting for CarParams") - CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) - VM = VehicleModel(CP) - cloudlog.info("Parameter learner got CarParams: %s" % CP.carFingerprint) - - params = params_reader.get("LiveParameters") - - # Check if car model matches - if params is not None: - params = json.loads(params) - if params.get('carFingerprint', None) != CP.carFingerprint: - cloudlog.info("Parameter learner found parameters for wrong car.") - params = None - - if params is None: - params = { - 'carFingerprint': CP.carFingerprint, - 'angleOffsetAverage': 0.0, - 'stiffnessFactor': 1.0, - 'steerRatio': VM.sR, - } - cloudlog.info("Parameter learner resetting to default values") - - cloudlog.info("Parameter starting with: %s" % str(params)) - localizer = Localizer(disabled_logs=disabled_logs) - - learner = ParamsLearner(VM, - angle_offset=params['angleOffsetAverage'], - stiffness_factor=params['stiffnessFactor'], - steer_ratio=params['steerRatio'], - learning_rate=LEARNING_RATE) - - i = 0 - while True: - for socket, event in poller.poll(timeout=1000): - log = messaging.recv_one(socket) - localizer.handle_log(log) - - if socket is car_state_socket: - if not localizer.kf.t: - continue - - if i % LEARNING_RATE == 0: - # carState is not updating the Kalman Filter, so update KF manually - localizer.kf.predict(1e-9 * log.logMonoTime) - - predicted_state = localizer.kf.x - yaw_rate = -float(predicted_state[5]) - - steering_angle = math.radians(log.carState.steeringAngle) - params_valid = learner.update(yaw_rate, log.carState.vEgo, steering_angle) - - params = messaging.new_message() - params.init('liveParameters') - params.liveParameters.valid = bool(params_valid) - params.liveParameters.angleOffset = float(math.degrees(learner.ao)) - params.liveParameters.angleOffsetAverage = float(math.degrees(learner.slow_ao)) - params.liveParameters.stiffnessFactor = float(learner.x) - params.liveParameters.steerRatio = float(learner.sR) - live_parameters_socket.send(params.to_bytes()) - - if i % 6000 == 0: # once a minute - params = learner.get_values() - params['carFingerprint'] = CP.carFingerprint - params_reader.put("LiveParameters", json.dumps(params)) - - i += 1 - elif socket is camera_odometry_socket: - msg = messaging.new_message() - msg.init('kalmanOdometry') - msg.logMonoTime = log.logMonoTime - msg.kalmanOdometry = localizer.liveLocationMsg(log.logMonoTime * 1e-9) - kalman_odometry_socket.send(msg.to_bytes()) - elif socket is sensor_events_socket: - pass - - -def main(gctx=None, addr="127.0.0.1"): - IN_CAR = os.getenv("IN_CAR", False) - disabled_logs = os.getenv("DISABLED_LOGS", "").split(",") - - # No speed for now - disabled_logs.append('carState') - if IN_CAR: - addr = "192.168.5.11" - - locationd_thread(gctx, addr, disabled_logs) - - -if __name__ == "__main__": - main() +#!/usr/bin/env python +import os +import zmq +import math +import json + +os.environ["OMP_NUM_THREADS"] = "1" +import numpy as np +from bisect import bisect_right + +from cereal import car +from common.params import Params +from common.numpy_fast import clip +import selfdrive.messaging as messaging +from selfdrive.swaglog import cloudlog +from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.services import service_list +from selfdrive.locationd.kalman.loc_local_kf import LocLocalKalman +from selfdrive.locationd.kalman.kalman_helpers import ObservationKind + +DEBUG = False +kf = LocLocalKalman() # Make sure that model is generated on import time + +MAX_ANGLE_OFFSET = math.radians(10.) +MAX_ANGLE_OFFSET_TH = math.radians(9.) +MIN_STIFFNESS = 0.5 +MAX_STIFFNESS = 2.0 +MIN_SR = 0.5 +MAX_SR = 2.0 +MIN_SR_TH = 0.55 +MAX_SR_TH = 1.9 + +LEARNING_RATE = 3 + +class Localizer(object): + def __init__(self, disabled_logs=None, dog=None): + self.kf = LocLocalKalman() + self.reset_kalman() + + self.max_age = .2 # seconds + self.calibration_valid = False + + if disabled_logs is None: + self.disabled_logs = list() + else: + self.disabled_logs = disabled_logs + + def reset_kalman(self): + self.filter_time = None + self.observation_buffer = [] + self.converter = None + self.speed_counter = 0 + self.sensor_counter = 0 + + def liveLocationMsg(self, time): + fix = messaging.log.KalmanOdometry.new_message() + + predicted_state = self.kf.x + fix.trans = [float(predicted_state[0]), float(predicted_state[1]), float(predicted_state[2])] + fix.rot = [float(predicted_state[3]), float(predicted_state[4]), float(predicted_state[5])] + + return fix + + def update_kalman(self, time, kind, meas): + idx = bisect_right([x[0] for x in self.observation_buffer], time) + self.observation_buffer.insert(idx, (time, kind, meas)) + while self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age: + self.kf.predict_and_observe(*self.observation_buffer.pop(0)) + + def handle_cam_odo(self, log, current_time): + self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, np.concatenate([log.cameraOdometry.rot, + log.cameraOdometry.rotStd])) + self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans, + log.cameraOdometry.transStd])) + + def handle_car_state(self, log, current_time): + self.speed_counter += 1 + if self.speed_counter % 5 == 0: + self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.carState.vEgo])) + + def handle_sensors(self, log, current_time): + for sensor_reading in log.sensorEvents: + # TODO does not yet account for double sensor readings in the log + if sensor_reading.type == 4: + self.sensor_counter += 1 + if self.sensor_counter % LEARNING_RATE == 0: + self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-sensor_reading.gyro.v[2], -sensor_reading.gyro.v[1], -sensor_reading.gyro.v[0]]) + + def handle_log(self, log): + current_time = 1e-9 * log.logMonoTime + typ = log.which + if typ in self.disabled_logs: + return + if typ == "sensorEvents": + self.handle_sensors(log, current_time) + elif typ == "carState": + self.handle_car_state(log, current_time) + elif typ == "cameraOdometry": + self.handle_cam_odo(log, current_time) + + +class ParamsLearner(object): + def __init__(self, VM, angle_offset=0., stiffness_factor=1.0, steer_ratio=None, learning_rate=1.0): + self.VM = VM + + self.ao = math.radians(angle_offset) + self.slow_ao = math.radians(angle_offset) + self.x = stiffness_factor + self.sR = VM.sR if steer_ratio is None else steer_ratio + self.MIN_SR = MIN_SR * self.VM.sR + self.MAX_SR = MAX_SR * self.VM.sR + self.MIN_SR_TH = MIN_SR_TH * self.VM.sR + self.MAX_SR_TH = MAX_SR_TH * self.VM.sR + + self.alpha1 = 0.01 * learning_rate + self.alpha2 = 0.00025 * learning_rate + self.alpha3 = 0.1 * learning_rate + self.alpha4 = 1.0 * learning_rate + + def get_values(self): + return { + 'angleOffsetAverage': math.degrees(self.slow_ao), + 'stiffnessFactor': self.x, + 'steerRatio': self.sR, + } + + def update(self, psi, u, sa): + cF0 = self.VM.cF + cR0 = self.VM.cR + aR = self.VM.aR + aF = self.VM.aF + l = self.VM.l + m = self.VM.m + + x = self.x + ao = self.ao + sR = self.sR + + # Gradient descent: learn angle offset, tire stiffness and steer ratio. + if u > 10.0 and abs(math.degrees(sa)) < 15.: + self.ao -= self.alpha1 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2) + + ao = self.slow_ao + self.slow_ao -= self.alpha2 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2) + + self.x -= self.alpha3 * -2.0*cF0*cR0*l*m*u**3*(ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**3) + + self.sR -= self.alpha4 * -2.0*cF0*cR0*l*u*x*(ao - sa)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**3*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2) + + if DEBUG: + # s1 = "Measured yaw rate % .6f" % psi + # ao = 0. + # s2 = "Uncompensated yaw % .6f" % (1.0*u*(-ao + sa)/(l*sR*(1 - m*u**2*(aF*cF0*x - aR*cR0*x)/(cF0*cR0*l**2*x**2)))) + # instant_ao = aF*m*psi*sR*u/(cR0*l*x) - aR*m*psi*sR*u/(cF0*l*x) - l*psi*sR/u + sa + s4 = "Instant AO: % .2f Avg. AO % .2f" % (math.degrees(self.ao), math.degrees(self.slow_ao)) + s5 = "Stiffnes: % .3f x" % self.x + print s4, s5 + + + self.ao = clip(self.ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET) + self.slow_ao = clip(self.slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET) + self.x = clip(self.x, MIN_STIFFNESS, MAX_STIFFNESS) + self.sR = clip(self.sR, self.MIN_SR, self.MAX_SR) + + # don't check stiffness for validity, as it can change quickly if sR is off + valid = abs(self.slow_ao) < MAX_ANGLE_OFFSET_TH and \ + self.sR > self.MIN_SR_TH and self.sR < self.MAX_SR_TH + + return valid + + +def locationd_thread(gctx, addr, disabled_logs): + ctx = zmq.Context() + poller = zmq.Poller() + + car_state_socket = messaging.sub_sock(ctx, service_list['carState'].port, poller, addr=addr, conflate=True) + sensor_events_socket = messaging.sub_sock(ctx, service_list['sensorEvents'].port, poller, addr=addr, conflate=True) + camera_odometry_socket = messaging.sub_sock(ctx, service_list['cameraOdometry'].port, poller, addr=addr, conflate=True) + path_plan_socket = messaging.sub_sock(ctx, service_list['pathPlan'].port, poller, addr=addr, conflate=True) + + kalman_odometry_socket = messaging.pub_sock(ctx, service_list['kalmanOdometry'].port) + live_parameters_socket = messaging.pub_sock(ctx, service_list['liveParameters'].port) + + params_reader = Params() + cloudlog.info("Parameter learner is waiting for CarParams") + CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) + VM = VehicleModel(CP) + cloudlog.info("Parameter learner got CarParams: %s" % CP.carFingerprint) + + params = params_reader.get("LiveParameters") + + # Check if car model matches + if params is not None: + params = json.loads(params) + if params.get('carFingerprint', None) != CP.carFingerprint: + cloudlog.info("Parameter learner found parameters for wrong car.") + params = None + + if params is None: + params = { + 'carFingerprint': CP.carFingerprint, + 'angleOffsetAverage': 0.0, + 'stiffnessFactor': 1.0, + 'steerRatio': VM.sR, + 'laneWidth': 3.6, + } + + try: + lane_width = params['laneWidth'] + except: + params['laneWidth'] = 3.6 + + cloudlog.info("Parameter learner resetting to default values") + + cloudlog.info("Parameter starting with: %s" % str(params)) + localizer = Localizer(disabled_logs=disabled_logs) + + learner = ParamsLearner(VM, + angle_offset=params['angleOffsetAverage'], + stiffness_factor=params['stiffnessFactor'], + steer_ratio=params['steerRatio'], + learning_rate=LEARNING_RATE) + + i = 0 + while True: + for socket, event in poller.poll(timeout=1000): + log = messaging.recv_one(socket) + localizer.handle_log(log) + + if socket is path_plan_socket: + lane_width = float(log.pathPlan.laneWidth) + + if socket is car_state_socket: + if not localizer.kf.t: + continue + + if i % LEARNING_RATE == 0: + # carState is not updating the Kalman Filter, so update KF manually + localizer.kf.predict(1e-9 * log.logMonoTime) + + predicted_state = localizer.kf.x + yaw_rate = -float(predicted_state[5]) + + steering_angle = math.radians(log.carState.steeringAngle) + params_valid = learner.update(yaw_rate, log.carState.vEgo, steering_angle) + + params = messaging.new_message() + params.init('liveParameters') + params.liveParameters.valid = bool(params_valid) + params.liveParameters.angleOffset = float(math.degrees(learner.ao)) + params.liveParameters.angleOffsetAverage = float(math.degrees(learner.slow_ao)) + params.liveParameters.stiffnessFactor = float(learner.x) + params.liveParameters.steerRatio = float(learner.sR) + params.liveParameters.laneWidth = float(lane_width) + live_parameters_socket.send(params.to_bytes()) + + if i % 6000 == 0: # once a minute + params = learner.get_values() + params['carFingerprint'] = CP.carFingerprint + params['laneWidth'] = float(lane_width) + params_reader.put("LiveParameters", json.dumps(params)) + + i += 1 + elif socket is camera_odometry_socket: + msg = messaging.new_message() + msg.init('kalmanOdometry') + msg.logMonoTime = log.logMonoTime + msg.kalmanOdometry = localizer.liveLocationMsg(log.logMonoTime * 1e-9) + kalman_odometry_socket.send(msg.to_bytes()) + elif socket is sensor_events_socket: + pass + + +def main(gctx=None, addr="127.0.0.1"): + IN_CAR = os.getenv("IN_CAR", False) + disabled_logs = os.getenv("DISABLED_LOGS", "").split(",") + + # No speed for now + disabled_logs.append('carState') + if IN_CAR: + addr = "192.168.5.11" + + locationd_thread(gctx, addr, disabled_logs) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/tune.py b/selfdrive/tune.py new file mode 100644 index 00000000000000..e9498a7a14082b --- /dev/null +++ b/selfdrive/tune.py @@ -0,0 +1,181 @@ +from selfdrive.kegman_conf import kegman_conf + +letters = { "a":[ "###", "# #", "###", "# #", "# #"], "b":[ "###", "# #", "###", "# #", "###"], "c":[ "###", "#", "#", "#", "###"], "d":[ "##", "# #", "# #", "# #", "##"], "e":[ "###", "#", "###", "#", "###"], "f":[ "###", "#", "###", "#", "#"], "g":[ "###", "# #", "###", " #", "###"], "h":[ "# #", "# #", "###", "# #", "# #"], "i":[ "###", " #", " #", " #", "###"], "j":[ "###", " #", " #", " #", "##"], "k":[ "# #", "##", "#", "##", "# #"], "l":[ "#", "#", "#", "#", "###"], "m":[ "# #", "###", "###", "# #", "# #"], "n":[ "###", "# #", "# #", "# #", "# #"], "o":[ "###", "# #", "# #", "# #", "###"], "p":[ "###", "# #", "###", "#", "#"], "q":[ "###", "# #", "###", " #", " #"], "r":[ "###", "# #", "##", "# #", "# #"], "s":[ "###", "#", "###", " #", "###"], "t":[ "###", " #", " #", " #", " #"], "u":[ "# #", "# #", "# #", "# #", "###"], "v":[ "# #", "# #", "# #", "# #", " #"], "w":[ "# #", "# #", "# #", "###", "###"], "x":[ "# #", " #", " #", " #", "# #"], "y":[ "# #", "# #", "###", " #", "###"], "z":[ "###", " #", " #", "#", "###"], " ":[ " "], "1":[ " #", "##", " #", " #", "###"], "2":[ "###", " #", "###", "#", "###"], "3":[ "###", " #", "###", " #", "###"], "4":[ "#", "#", "# #", "###", " #"], "5":[ "###", "#", "###", " #", "###"], "6":[ "###", "#", "###", "# #", "###"], "7":[ "###", " # ", " #", " #", "#"], "8":[ "###", "# #", "###", "# #", "###"], "9":[ "###", "# #", "###", " #", "###"], "0":[ "###", "# #", "# #", "# #", "###"], "!":[ " # ", " # ", " # ", " ", " # "], "?":[ "###", " #", " ##", " ", " # "], ".":[ " ", " ", " ", " ", " # "], "]":[ " ", " ", " ", " #", " # "], "/":[ " #", " #", " # ", "# ", "# "], ":":[ " ", " # ", " ", " # ", " "], "@":[ "###", "# #", "## ", "# ", "###"], "'":[ " # ", " # ", " ", " ", " "], "#":[ " # ", "###", " # ", "###", " # "], "-":[ " ", " ","###"," "," "] } +# letters stolen from here: http://www.stuffaboutcode.com/2013/08/raspberry-pi-minecraft-twitter.html + +def print_letters(text): + bigletters = [] + for i in text: + bigletters.append(letters.get(i.lower(),letters[' '])) + output = ['']*5 + for i in range(5): + for j in bigletters: + temp = ' ' + try: + temp = j[i] + except: + pass + temp += ' '*(5-len(temp)) + temp = temp.replace(' ',' ') + temp = temp.replace('#','@') + output[i] += temp + return '\n'.join(output) +import sys, termios, tty, os, time + +def getch(): + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +button_delay = 0.2 + +kegman = kegman_conf() +#kegman.conf['tuneGernby'] = "1" +#kegman.write_config(kegman.conf) +param = ["tuneGernby", "reactMPC", "dampMPC", "reactSteer", "dampSteer", "rateFF", "angleFF", "Kp", "Ki"] + +j = 0 +while True: + print "" + print print_letters(param[j][0:9]) + print "" + print print_letters(kegman.conf[param[j]]) + print "" + print "reactMPC is an adjustment to the time projection of the MPC" + print "angle used in the dampening calculation. Increasing this value" + print "would cause the vehicle to turn sooner." + print "" + print "" + print "reactSteer is an adjustment to the time projection of steering" + print "rate to determine future steering angle. If the steering is " + print "too shaky, decrease this value (may be negative). If the" + print "steering response is too slow, increase this value." + print "" + print "" + print "dampMPC / dampSteer is the amount of time that the samples" + print "will be projected and averaged to smooth the values" + print "" + print "" + print ("Press 1, 3, 5, 7 to incr 0.1, 0.05, 0.01, 0.001") + print ("press a, d, g, j to decr 0.1, 0.05, 0.01, 0.001") + print ("press 0 / L to make the value 0 / 1") + print ("press SPACE / m for next /prev parameter") + print ("press q to quit") + + char = getch() + write_json = False + if (char == "7"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.001) + write_json = True + + if (char == "5"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.01) + write_json = True + + elif (char == "3"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.05) + write_json = True + + elif (char == "1"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.1) + write_json = True + + elif (char == "j"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.001) + write_json = True + + elif (char == "g"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.01) + write_json = True + + elif (char == "d"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.05) + write_json = True + + elif (char == "a"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.1) + write_json = True + + elif (char == "0"): + kegman.conf[param[j]] = "0" + write_json = True + + elif (char == "l"): + kegman.conf[param[j]] = "1" + write_json = True + + elif (char == " "): + if j < len(param) - 1: + j = j + 1 + else: + j = 0 + + elif (char == "m"): + if j > 0: + j = j - 1 + else: + j = len(param) - 1 + + elif (char == "q"): + break + + + if float(kegman.conf['tuneGernby']) != 1 and float(kegman.conf['tuneGernby']) != 0: + kegman.conf['tuneGernby'] = "0" + + if float(kegman.conf['dampSteer']) < 0 and float(kegman.conf['dampSteer']) != -1: + kegman.conf['dampSteer'] = "0" + + if float(kegman.conf['dampMPC']) < 0 and float(kegman.conf['dampMPC']) != -1: + kegman.conf['dampMPC'] = "0" + + if float(kegman.conf['rateFF']) < 0: + kegman.conf['rateFF'] = "0.0" + + if float(kegman.conf['angleFF']) < 0: + kegman.conf['angleFF'] = "0.0" + + if float(kegman.conf['dampMPC']) > 1.0: + kegman.conf['dampMPC'] = "1.0" + + if float(kegman.conf['dampSteer']) > 1.0: + kegman.conf['dampSteer'] = "1.0" + + if float(kegman.conf['reactMPC']) < -0.99 and float(kegman.conf['reactMPC']) != -1: + kegman.conf['reactMPC'] = "-0.99" + + if float(kegman.conf['reactMPC']) > 1.0: + kegman.conf['reactMPC'] = "1.0" + + if float(kegman.conf['reactSteer']) < -0.99 and float(kegman.conf['reactSteer']) != -1: + kegman.conf['reactSteer'] = "-0.99" + + if float(kegman.conf['reactSteer']) > 1.0: + kegman.conf['reactSteer'] = "1.0" + + if float(kegman.conf['Ki']) < 0 and float(kegman.conf['Ki']) != -1: + kegman.conf['Ki'] = "0" + + if float(kegman.conf['Ki']) > 2: + kegman.conf['Ki'] = "2" + + if float(kegman.conf['Kp']) < 0 and float(kegman.conf['Kp']) != -1: + kegman.conf['Kp'] = "0" + + if float(kegman.conf['Kp']) > 3: + kegman.conf['Kp'] = "3" + + + + + + if write_json: + kegman.write_config(kegman.conf) + + time.sleep(button_delay) diff --git a/selfdrive/visiond/visiond.cc b/selfdrive/visiond/visiond.cc index af10d760c132e8..24f98f875a9515 100644 --- a/selfdrive/visiond/visiond.cc +++ b/selfdrive/visiond/visiond.cc @@ -1129,7 +1129,7 @@ void* processing_thread(void *arg) { //printf("avg %f\n", pose_output[0]); posenet->execute(posenet_input); - + // fix stddevs for (int i = 6; i < 12; i++) { pose_output[i] = log1p(exp(pose_output[i])) + 1e-6; diff --git a/tune.py b/tune.py new file mode 100644 index 00000000000000..e29c8f728a64e6 --- /dev/null +++ b/tune.py @@ -0,0 +1,175 @@ +from selfdrive.kegman_conf import kegman_conf + +letters = { "a":[ "###", "# #", "###", "# #", "# #"], "b":[ "###", "# #", "###", "# #", "###"], "c":[ "###", "#", "#", "#", "###"], "d":[ "##", "# #", "# #", "# #", "##"], "e":[ "###", "#", "###", "#", "###"], "f":[ "###", "#", "###", "#", "#"], "g":[ "###", "# #", "###", " #", "###"], "h":[ "# #", "# #", "###", "# #", "# #"], "i":[ "###", " #", " #", " #", "###"], "j":[ "###", " #", " #", " #", "##"], "k":[ "# #", "##", "#", "##", "# #"], "l":[ "#", "#", "#", "#", "###"], "m":[ "# #", "###", "###", "# #", "# #"], "n":[ "###", "# #", "# #", "# #", "# #"], "o":[ "###", "# #", "# #", "# #", "###"], "p":[ "###", "# #", "###", "#", "#"], "q":[ "###", "# #", "###", " #", " #"], "r":[ "###", "# #", "##", "# #", "# #"], "s":[ "###", "#", "###", " #", "###"], "t":[ "###", " #", " #", " #", " #"], "u":[ "# #", "# #", "# #", "# #", "###"], "v":[ "# #", "# #", "# #", "# #", " #"], "w":[ "# #", "# #", "# #", "###", "###"], "x":[ "# #", " #", " #", " #", "# #"], "y":[ "# #", "# #", "###", " #", "###"], "z":[ "###", " #", " #", "#", "###"], " ":[ " "], "1":[ " #", "##", " #", " #", "###"], "2":[ "###", " #", "###", "#", "###"], "3":[ "###", " #", "###", " #", "###"], "4":[ "#", "#", "# #", "###", " #"], "5":[ "###", "#", "###", " #", "###"], "6":[ "###", "#", "###", "# #", "###"], "7":[ "###", " # ", " #", " #", "#"], "8":[ "###", "# #", "###", "# #", "###"], "9":[ "###", "# #", "###", " #", "###"], "0":[ "###", "# #", "# #", "# #", "###"], "!":[ " # ", " # ", " # ", " ", " # "], "?":[ "###", " #", " ##", " ", " # "], ".":[ " ", " ", " ", " ", " # "], "]":[ " ", " ", " ", " #", " # "], "/":[ " #", " #", " # ", "# ", "# "], ":":[ " ", " # ", " ", " # ", " "], "@":[ "###", "# #", "## ", "# ", "###"], "'":[ " # ", " # ", " ", " ", " "], "#":[ " # ", "###", " # ", "###", " # "], "-":[ " ", " ","###"," "," "] } +# letters stolen from here: http://www.stuffaboutcode.com/2013/08/raspberry-pi-minecraft-twitter.html + +def print_letters(text): + bigletters = [] + for i in text: + bigletters.append(letters.get(i.lower(),letters[' '])) + output = ['']*5 + for i in range(5): + for j in bigletters: + temp = ' ' + try: + temp = j[i] + except: + pass + temp += ' '*(5-len(temp)) + temp = temp.replace(' ',' ') + temp = temp.replace('#','\xE2\x96\x88') + output[i] += temp + return '\n'.join(output) +import sys, termios, tty, os, time + +def getch(): + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +button_delay = 0.2 + +kegman = kegman_conf() +#kegman.conf['tuneGernby'] = "1" +#kegman.write_config(kegman.conf) +param = ["tuneGernby", "reactMPC", "dampMPC", "rateFF", "angleFF", "Kp", "Ki"] + +j = 0 +while True: + print "" + print print_letters(param[j][0:9]) + print "" + print print_letters(kegman.conf[param[j]]) + print "" + print "reactMPC is an adjustment to the time projection of the MPC" + print "angle used in the dampening calculation. Increasing this value" + print "would cause the vehicle to turn sooner." + print "" + print "" + print "dampMPC is the amount of time that the samples" + print "will be projected and averaged to smooth the values" + print "" + print "" + print ("Press 1, 3, 5, 7 to incr 0.1, 0.05, 0.01, 0.001") + print ("press a, d, g, j to decr 0.1, 0.05, 0.01, 0.001") + print ("press 0 / L to make the value 0 / 1") + print ("press SPACE / m for next /prev parameter") + print ("press z to quit") + + char = getch() + write_json = False + if (char == "7"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.001) + write_json = True + + if (char == "5"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.01) + write_json = True + + elif (char == "3"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.05) + write_json = True + + elif (char == "1"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.1) + write_json = True + + elif (char == "j"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.001) + write_json = True + + elif (char == "g"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.01) + write_json = True + + elif (char == "d"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.05) + write_json = True + + elif (char == "a"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.1) + write_json = True + + elif (char == "0"): + kegman.conf[param[j]] = "0" + write_json = True + + elif (char == "l"): + kegman.conf[param[j]] = "1" + write_json = True + + elif (char == " "): + if j < len(param) - 1: + j = j + 1 + else: + j = 0 + + elif (char == "m"): + if j > 0: + j = j - 1 + else: + j = len(param) - 1 + + elif (char == "z"): + break + + + if float(kegman.conf['tuneGernby']) != 1 and float(kegman.conf['tuneGernby']) != 0: + kegman.conf['tuneGernby'] = "0" + + if float(kegman.conf['dampSteer']) < 0 and float(kegman.conf['dampSteer']) != -1: + kegman.conf['dampSteer'] = "0" + + if float(kegman.conf['dampMPC']) < 0 and float(kegman.conf['dampMPC']) != -1: + kegman.conf['dampMPC'] = "0" + + if float(kegman.conf['rateFF']) < 0: + kegman.conf['rateFF'] = "0.0" + + if float(kegman.conf['angleFF']) < 0: + kegman.conf['angleFF'] = "0.0" + + if float(kegman.conf['dampMPC']) > 1.0: + kegman.conf['dampMPC'] = "1.0" + + if float(kegman.conf['dampSteer']) > 1.0: + kegman.conf['dampSteer'] = "1.0" + + if float(kegman.conf['reactMPC']) < -0.99 and float(kegman.conf['reactMPC']) != -1: + kegman.conf['reactMPC'] = "-0.99" + + if float(kegman.conf['reactMPC']) > 1.0: + kegman.conf['reactMPC'] = "1.0" + + if float(kegman.conf['reactSteer']) < -0.99 and float(kegman.conf['reactSteer']) != -1: + kegman.conf['reactSteer'] = "-0.99" + + if float(kegman.conf['reactSteer']) > 1.0: + kegman.conf['reactSteer'] = "1.0" + + if float(kegman.conf['Ki']) < 0 and float(kegman.conf['Ki']) != -1: + kegman.conf['Ki'] = "0" + + if float(kegman.conf['Ki']) > 2: + kegman.conf['Ki'] = "2" + + if float(kegman.conf['Kp']) < 0 and float(kegman.conf['Kp']) != -1: + kegman.conf['Kp'] = "0" + + if float(kegman.conf['Kp']) > 3: + kegman.conf['Kp'] = "3" + + + + + + if write_json: + kegman.write_config(kegman.conf) + + time.sleep(button_delay)