diff --git a/scripts/daq/.gitignore b/scripts/daq/.gitignore new file mode 100644 index 000000000..e12f48a3f --- /dev/null +++ b/scripts/daq/.gitignore @@ -0,0 +1,3 @@ +generated/ +can_cache.sqlite +logs/ \ No newline at end of file diff --git a/scripts/daq/README.md b/scripts/daq/README.md new file mode 100644 index 000000000..fa674185c --- /dev/null +++ b/scripts/daq/README.md @@ -0,0 +1,12 @@ +This is just a temporary working directory for daq... + +Generate Go DBC bindings: +``` +go get -u go.einride.tech/can +cd dbc +go run go.einride.tech/can/cmd/cantool generate ./ ../generated/ +``` + +Had to remove the "Reset" message from DashCommand to avoid name conflicts with bindings +> TODO: either find a better Go DBC binding generator or patch the upstream DBC to +> avoid having messages named "Reset" \ No newline at end of file diff --git a/scripts/daq/can.go b/scripts/daq/can.go new file mode 100644 index 000000000..1f2246651 --- /dev/null +++ b/scripts/daq/can.go @@ -0,0 +1,75 @@ +package main + +import ( + "fmt" + "time" + + "github.com/macformula/hil/canlink" + "go.einride.tech/can" + "go.einride.tech/can/pkg/generated" + "go.uber.org/zap" +) + +const ( + // MAX_FRAME_INTERVAL is the milliseconds between using identical CAN frames + MAX_FRAME_INTERVAL = 100 +) + +type DbcMessagesDescriptor interface { + UnmarshalFrame(f can.Frame) (generated.Message, error) +} + +type DataAcquisitionHandler struct { + md DbcMessagesDescriptor + telemetry *TelemetryHandler + busName string + l *zap.Logger + frameArrivalMap map[uint32]time.Time +} + +func NewDaqHandler(md DbcMessagesDescriptor, telemetry *TelemetryHandler, busName string, l *zap.Logger) *DataAcquisitionHandler { + return &DataAcquisitionHandler{ + md: md, + l: l, + telemetry: telemetry, + busName: busName, + frameArrivalMap: map[uint32]time.Time{}, + } +} + +func (d *DataAcquisitionHandler) Name() string { + return fmt.Sprintf("DAQ Handler") +} + +func (d *DataAcquisitionHandler) Handle(broadcastChan chan canlink.TimestampedFrame, stopChan chan struct{}) error { + for { + select { + case <-stopChan: + d.l.Info("stopping handle") + case receivedFrame := <-broadcastChan: + // CAN frame received, parse it and queue it for transmission or file caching + + // We wont do any additional parsing for now, but it might be useful later + /* + * msg, err := d.md.UnmarshalFrame(receivedFrame.Frame) + * if err != nil { + * return errors.Wrap(err, "daq: handle:") + * } + */ + + fmt.Printf("daq: received frame: %s\n", receivedFrame.Frame.String()) + lastSeen, exists := d.frameArrivalMap[receivedFrame.Frame.ID] + + // Frame was already seen less than MAX_FRAME_INTERVAL ms ago, ignore it + if exists && time.Now().Sub(lastSeen).Milliseconds() < MAX_FRAME_INTERVAL { + break + } + + err := d.telemetry.Enqueue(receivedFrame, d.busName) + if err != nil { + fmt.Printf("daq: failed to enqueue frame: %s\n", err.Error()) + } + default: + } + } +} diff --git a/scripts/daq/dbc/veh.dbc b/scripts/daq/dbc/veh.dbc new file mode 100644 index 000000000..b0a9d44c2 --- /dev/null +++ b/scripts/daq/dbc/veh.dbc @@ -0,0 +1,402 @@ +VERSION "" + + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: + +BU_: FC LVC TMS DASH RPI IMD BMS GPS + +CM_ "ID Scheme: +Replace x with the pertinent ECU (not necessarily the sender) +x=0 Front Controller +x=1 LV Controller +x=2 TMS +x=3 Dashboard +x=4 RPI + +Increment n as needed"; + +CM_ "1xn - Irregular, high importance commands"; + +BO_ 140 InitiateCanFlash: 1 RPI + SG_ ECU : 0|8@1+ (1,0) [0|2] "" FC, LVC, TMS + +VAL_ 140 ECU 0 "FrontController" 1 "LvController" 2 "TMS"; + +CM_ "Periodic Messages +2x0 - ECUx Commands +2x1 - ECUx Statuses +2x2 - ECUx Alerts + +Each 2x1 status should contain an 8-bit counter field which increments +on each transmission to show that the ECU is alive."; + +BO_ 201 FcStatus: 8 FC + SG_ Counter : 0|8@1+ (1,0) [0|255] "" RPI + SG_ State : 8|8@1+ (1,0) [0|255] "" RPI + SG_ AccumulatorState : 16|8@1+ (1,0) [0|255] "" RPI + SG_ MotorState : 24|8@1+ (1,0) [0|255] "" RPI + SG_ Inv1State : 32|4@1+ (1,0) [0|255] "" RPI + SG_ Inv2State : 36|4@1+ (1,0) [0|255] "" RPI + SG_ DbcValid : 40|1@1+ (1,0) [0|1] "" RPI + SG_ Inv1Starter : 48|4@1+ (1,0) [0|255] "" RPI + SG_ Inv2Starter : 52|4@1+ (1,0) [0|255] "" RPI + +VAL_ 201 State 0 "START_DASHBOARD" 2 "WAIT_DRIVER_SELECT" 3 "WAIT_START_HV" 4 "STARTING_HV" 5 "WAIT_START_MOTOR" 6 "STARTING_MOTORS" 7 "STARTUP_SEND_READY_TO_DRIVE" 8 "RUNNING" 9 "SHUTDOWN" 10 "ERROR"; +VAL_ 201 MotorState 0 "IDLE" 1 "STARTING" 2 "SWITCHING_INVERTER_ON" 3 "RUNNING" 4 "ERROR"; +VAL_ 201 AccumulatorState 0 "IDLE" 1 "STARTUP_ENSURE_OPEN" 2 "STARTUP_CLOSE_NEG" 3 "STARTUP_HOLD_CLOSE_NEG" 4 "STARTUP_CLOSE_PRECHARGE" 5 "STARTUP_HOLD_CLOSE_PRECHARGE" 6 "STARTUP_CLOSE_POS" 7 "STARTUP_HOLD_CLOSE_POS" 8 "STARTUP_OPEN_PRECHARGE" 9 "RUNNING" 10 "SHUTDOWN" 11 "ERROR"; +VAL_ 201 Inv1State 0 "OFF" 1 "SYSTEM_READY" 2 "STARTUP_bDCON" 3 "STARTUP_bENABLE" 4 "STARTUP_bINVERTER" 5 "STARTUP_X140" 6 "RUNNING" 7 "ERROR" 8 "ERROR_RESET"; +VAL_ 201 Inv2State 0 "OFF" 1 "SYSTEM_READY" 2 "STARTUP_bDCON" 3 "STARTUP_bENABLE" 4 "STARTUP_bINVERTER" 5 "STARTUP_X140" 6 "RUNNING" 7 "ERROR" 8 "ERROR_RESET"; + +BO_ 202 FcAlerts: 2 FC + SG_ AppsImplausible : 0|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ AccumulatorLowSoc : 1|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ AccumulatorContactorWrongState : 2|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ MotorRetriesExceeded : 3|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ LeftMotorStartingError : 4|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ RightMotorStartingError : 5|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ LeftMotorRunningError : 6|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ RightMotorRunningError : 7|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ DashboardBootTimeout : 8|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ CanTxError : 9|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ EV47Active : 10|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ NoInv1Can : 11|1@1+ (1,0) [0|1] "" RPI, DASH + SG_ NoInv2Can : 12|1@1+ (1,0) [0|1] "" RPI, DASH + +BO_ 203 FcCounters: 8 FC + SG_ Motor : 0|8@1+ (1,0) [0|1] "" RPI, DASH + SG_ Amk1: 8|8@1+ (1,0) [0|1] "" RPI, DASH + SG_ Amk2 : 16|8@1+ (1,0) [0|1] "" RPI, DASH + SG_ Starter1 : 24|8@1+ (1,0) [0|1] "" RPI, DASH + SG_ Starter2 : 32|8@1+ (1,0) [0|1] "" RPI, DASH + +BO_ 210 LvCommand: 1 FC + SG_ BrakeLightEnable : 1|1@1+ (1,0) [0|1] "" LVC + +BO_ 213 InverterSwitchCommand: 1 FC + SG_ CloseInverterSwitch : 0|1@1+ (1,0) [0|1] "" LVC + +CM_ BO_ 213 "This should be combined with LvCommand once testing is done."; + +BO_ 211 LvStatus: 4 LVC + SG_ Counter : 0|8@1+ (1,0) [0|255] "" FC + SG_ LvState : 8|8@1+ (1,0) [0|1] "" FC + SG_ MotorControllerState : 16|8@1+ (1,0) [0|1] "" FC + SG_ MotorControllerSwitchClosed : 24|1@1+ (1,0) [0|1] "" FC + SG_ ImdFault : 25|1@1+ (1,0) [0|1] "" FC + SG_ BmsFault : 26|1@1+ (1,0) [0|1] "" FC + +VAL_ 211 LvState 0 "PWRUP_START" 1 "PWRUP_TSSI_ON" 2 "PWRUP_PERIPHERALS_ON" 3 "PWRUP_ACCUMULATOR_ON" 4 "PWRUP_MOTOR_CONTROLLER_PRECHARGING" 7 "PWRUP_SHUTDOWN_ON" 9 "DCDC_ON" 10 "POWERTRAIN_PUMP_ON" 11 "POWERTRAIN_FAN_ON" 13 "READY_TO_DRIVE" 14 "SHUTDOWN_DRIVER_WARNING" 15 "SHUTDOWN_PUMP_OFF" 16 "SHUTDOWN_FAN_OFF" 17 "SHUTDOWN_COMPLETE"; +VAL_ 211 MotorControllerState 0 "OFF" 1 "PRECHARGING" 2 "PRECHARGING_HANDOFF" 3 "ON"; + +BO_ 230 DashCommand: 5 FC + SG_ ConfigReceived : 0|1@1+ (1,0) [0|1] "" DASH + SG_ HvStarted : 1|1@1+ (1,0) [0|1] "" DASH + SG_ MotorStarted : 2|1@1+ (1,0) [0|1] "" DASH + SG_ DriveStarted : 3|1@1+ (1,0) [0|1] "" DASH + SG_ Errored : 5|1@1+ (1,0) [0|1] "" DASH + SG_ HvPrechargePercent : 8|8@1+ (1,0) [0|100] "" DASH + SG_ Speed : 16|12@1+ (0.1,0) [0|100] "mph" DASH + SG_ HvSocPercent : 32|8@1+ (1,0) [0|100] "" DASH + +BO_ 231 DashStatus: 3 DASH + SG_ Counter : 0|8@1+ (1,0) [0|255] "" FC + SG_ State : 8|8@1+ (1,0) [0|1] "" FC + SG_ Profile : 16|8@1+ (1,0) [0|15] "" FC + +VAL_ 231 State 0 "LOGO" 1 "SELECT_PROFILE" 2 "CONFIRM_SELECTION" 3 "WAIT_SELECTION_ACK" 4 "PRESS_FOR_HV" 5 "STARTING_HV" 6 "PRESS_FOR_MOTOR" 7 "STARTING_MOTORS" 8 "BRAKE_TO_START" 9 "RUNNING" 10 "SHUTDOWN" 11 "ERROR"; +VAL_ 231 Profile 0 "Default" 1 "Launch" 2 "Skidpad" 3 "Endurance" 4 "Tuning" 5 "_ENUM_TAIL_"; + +BO_ 300 Accumulator_Soc: 8 FC + SG_ PackVoltage : 0|16@1+ (1,0) [0|255] "" DASH + SG_ PrechargeVoltage : 16|16@1+ (1,0) [0|255] "" DASH + SG_ MaxPackVoltage : 32|16@1+ (1,0) [0|255] "" DASH + SG_ SocPercent : 48|8@1+ (1,0) [0|255] "" DASH + SG_ PrechargePercent : 56|8@1+ (1,0) [0|255] "" DASH + +CM_ "3xn - Additional sensor readings"; + +BO_ 310 SuspensionTravel34: 2 LVC + SG_ STP3 : 0|8@1+ (1,0) [0|255] "" FC + SG_ STP4 : 8|8@1+ (1,0) [0|255] "" FC + +BO_ 340 TuningParams: 8 RPI + SG_ aggressiveness : 0|8@1+ (1,0) [0|100] "" FC + +CM_ "4xn - General / debugging info"; + +BO_ 400 FcGitHash: 5 FC + SG_ Commit : 0|32@1+ (1,0) [0|0] "" RPI + SG_ Dirty : 32|1@1+ (1,0) [0|0] "" RPI + +BO_ 410 LvGitHash: 5 LVC + SG_ Commit : 0|32@1+ (1,0) [0|0] "" RPI + SG_ Dirty : 32|1@1+ (1,0) [0|0] "" RPI + +BO_ 420 TmsGitHash: 5 LVC + SG_ Commit : 0|32@1+ (1,0) [0|0] "" RPI + SG_ Dirty : 32|1@1+ (1,0) [0|0] "" RPI + +BO_ 430 DashGitHash: 5 DASH + SG_ Commit : 0|32@1+ (1,0) [0|0] "" RPI + SG_ Dirty : 32|1@1+ (1,0) [0|0] "" RPI + +BO_ 401 AppsDebug: 8 FC + SG_ Apps1RawVolt : 0|16@1+ (0.001,0) [0|0] "volt" RPI + SG_ Apps2RawVolt : 16|16@1+ (0.001,0) [0|0] "volt" RPI + SG_ Apps1Percent : 32|16@1+ (0.1,0) [0|0] "percent" RPI + SG_ Apps2Percent : 48|16@1+ (0.1,0) [0|0] "percent" RPI + +BO_ 402 BppsSteerDebug: 8 FC + SG_ BppsRawVolt : 0|16@1+ (0.001,0) [0|0] "volt" RPI + SG_ SteerRawVolt : 16|16@1+ (0.001,0) [0|0] "volt" RPI + SG_ BppsPercent : 32|16@1+ (0.1,0) [0|0] "percent" RPI + SG_ SteerPosition : 48|16@1+ (0.01,-1) [0|0] "[-1,+1]" RPI + +BO_ 411 LvDbcHash: 8 LVC + SG_ Hash : 0|64@1+ (1,0) [0|1] "" FC + +CM_ "Manufacturer specific IDs (do not modify) +55 IMD +769-777 GPS/IMU +1570-1574 BMS Command/Status +2553934720 BMS Temperatures 1 +2566844926 BMS Temperatures 2"; + +BO_ 55 IMD_Info_General: 8 IMD + SG_ rIsoCorrected : 0|16@1+ (1,0) [0|0] "" RPI + SG_ rIsoStatus : 16|8@1+ (1,0) [0|0] "" RPI + SG_ MeasurementCounter : 24|8@1+ (1,0) [0|0] "" RPI + SG_ WarningsAlarms : 32|16@1+ (1,0) [0|0] "" RPI + SG_ DeviceActivity : 48|8@1+ (1,0) [0|0] "" RPI + SG_ Reserved : 56|8@1+ (1,0) [0|0] "" RPI + +BO_ 56 IMD_InfoIsolationDetail: 8 IMD + SG_ rIsoNeg : 0|16@1+ (1,0) [0|0] "" RPI + SG_ rIsoPos : 16|16@1+ (1,0) [0|0] "" RPI + SG_ rIsoOriginal : 32|16@1+ (1,0) [0|0] "" RPI + SG_ isolationMeasurementCounter : 48|8@1+ (1,0) [0|0] "" RPI + SG_ isolationQuality : 56|8@1+ (1,0) [0|0] "" RPI + +BO_ 57 IMD_Info_Voltage: 8 IMD + SG_ hvSystem : 0|16@1+ (1,0) [0|0] "" RPI + SG_ hvNegToEarth : 16|16@1+ (1,0) [0|0] "" RPI + SG_ hvPosToEarth : 32|16@1+ (1,0) [0|0] "" RPI + SG_ voltageMeasurementCounter : 48|8@1+ (1,0) [0|0] "" RPI + SG_ Reserved : 56|8@1+ (1,0) [0|0] "" RPI + +BO_ 58 IMD_InfoItSystem: 8 IMD + SG_ capacityMeasuredValue : 0|16@1+ (1,0) [0|0] "" RPI + SG_ capacityMeasurementCounter : 16|8@1+ (1,0) [0|0] "" RPI + SG_ unbalanceMeasuredValue : 24|8@1+ (1,0) [0|0] "" RPI + SG_ unbalanceMeasurement : 32|8@1+ (1,0) [0|0] "" RPI + SG_ voltageMeasuredFrequency : 40|16@1+ (1,0) [0|0] "" RPI + SG_ Reserved : 56|8@1+ (1,0) [0|0] "" RPI + +BO_ 34 IMD_Request: 8 RPI + SG_ index : 0|8@1+ (1,0) [0|0] "" IMD + SG_ data0 : 8|8@1+ (1,0) [0|0] "" IMD + SG_ data1 : 16|8@1+ (1,0) [0|0] "" IMD + SG_ data2 : 24|8@1+ (1,0) [0|0] "" IMD + SG_ data3 : 32|8@1+ (1,0) [0|0] "" IMD + SG_ data4 : 40|8@1+ (1,0) [0|0] "" IMD + SG_ data5 : 48|8@1+ (1,0) [0|0] "" IMD + SG_ data6 : 56|8@1+ (1,0) [0|0] "" IMD + +BO_ 35 ID_Response: 8 IMD + SG_ index : 0|8@1+ (1,0) [0|0] "" RPI + SG_ d1 : 8|8@1+ (1,0) [0|0] "" RPI + SG_ d2 : 16|8@1+ (1,0) [0|0] "" RPI + SG_ d3 : 24|8@1+ (1,0) [0|0] "" RPI + SG_ d4 : 32|8@1+ (1,0) [0|0] "" RPI + SG_ d5 : 40|8@1+ (1,0) [0|0] "" RPI + SG_ d6 : 48|8@1+ (1,0) [0|0] "" RPI + SG_ d7 : 56|8@1+ (1,0) [0|0] "" RPI + +BO_ 1570 ContactorCommand: 3 FC + SG_ PackPositive : 0|8@1+ (1,0) [0|0] "" BMS + SG_ PackPrecharge : 8|8@1+ (1,0) [0|0] "" BMS + SG_ PackNegative : 16|8@1+ (1,0) [0|0] "" BMS + +BO_ 1572 Pack_State: 7 BMS + SG_ Pack_Current : 0|16@1+ (0.1,0) [0|0] "Amps" FC + SG_ Pack_Inst_Voltage : 16|16@1+ (0.1,0) [0|0] "Volts" FC + SG_ Avg_Cell_Voltage : 32|16@1+ (0.0001,0) [0|0] "Volts" FC + SG_ Populated_Cells : 48|8@1+ (1,0) [0|0] "Num" FC + +BO_ 1571 Pack_Current_Limits: 4 BMS + SG_ Pack_CCL : 0|16@1+ (1,0) [0|0] "Amps" FC + SG_ Pack_DCL : 16|16@1+ (1,0) [0|0] "Amps" FC + +BO_ 1573 Pack_SOC: 3 BMS + SG_ Pack_SOC : 0|8@1+ (0.5,0) [0|0] "Percent" FC + SG_ Maximum_Pack_Voltage : 8|16@1+ (0.1,0) [0|0] "Volts" FC + +BO_ 1574 Contactor_Feedback: 3 BMS + SG_ Pack_Positive_Feedback : 0|1@1+ (1,0) [0|1] "" FC, DASH, LVC + SG_ Pack_Negative_Feedback : 8|1@1+ (1,0) [0|1] "" FC, DASH, LVC + SG_ Pack_Precharge_Feedback : 16|1@1+ (1,0) [0|1] "" FC, DASH, LVC + +BO_ 2553934720 BmsBroadcast: 8 TMS + SG_ ThermModuleNum : 0|8@1+ (1,0) [0|0] "" BMS + SG_ LowThermValue : 8|8@1- (1,0) [0|0] " C" BMS + SG_ HighThermValue : 16|8@1- (1,0) [0|0] " C" BMS + SG_ AvgThermValue : 24|8@1- (1,0) [0|0] " C" BMS + SG_ NumThermEn : 32|8@1+ (1,0) [0|0] "" BMS + SG_ HighThermID : 40|8@1+ (1,0) [0|0] "" BMS + SG_ LowThermID : 48|8@1+ (1,0) [0|0] "" BMS + SG_ Checksum : 56|8@1+ (1,0) [0|0] "" BMS + +BO_ 2566844926 ThermistorBroadcast: 8 TMS + SG_ RelThermID : 0|16@1+ (1,0) [0|0] "" BMS + SG_ ThermValue : 16|8@1- (1,0) [0|0] " C" BMS + SG_ NumEnTherm : 24|8@1- (1,0) [0|0] "" BMS + SG_ LowThermValue : 32|8@1- (1,0) [0|0] " C" BMS + SG_ HighThermValue : 40|8@1- (1,0) [0|0] " C" BMS + SG_ HighThermID : 48|8@1+ (1,0) [0|0] "" BMS + SG_ LowThermID : 56|8@1+ (1,0) [0|0] "" BMS + +BO_ 769 GnssStatus: 1 GPS + SG_ FixType : 0|3@1+ (1,0) [0|5] "" RPI + SG_ Satellites : 3|5@1+ (1,0) [0|31] "" RPI + +BO_ 770 GnssTime: 6 GPS + SG_ TimeValid : 0|1@1+ (1,0) [0|1] "" RPI + SG_ TimeConfirmed : 1|1@1+ (1,0) [0|1] "" RPI + SG_ Epoch : 8|40@1+ (0.001,1577840400) [0|0] "sec" RPI + +BO_ 771 GnssPosition: 8 GPS + SG_ PositionValid : 0|1@1+ (1,0) [0|1] "" RPI + SG_ Latitude : 1|28@1+ (1E-006,-90) [-90|178.435455] "deg" RPI + SG_ Longitude : 29|29@1+ (1E-006,-180) [-180|356.870911] "deg" RPI + SG_ PositionAccuracy : 58|6@1+ (1,0) [0|63] "m" RPI + +BO_ 772 GnssAltitude: 4 GPS + SG_ AltitudeValid : 0|1@1+ (1,0) [0|1] "" RPI + SG_ Altitude : 1|18@1+ (0.1,-6000) [-6000|20000] "m" RPI + SG_ AltitudeAccuracy : 19|13@1+ (1,0) [0|8000] "m" RPI + +BO_ 773 GnssAttitude: 8 GPS + SG_ AttitudeValid : 0|1@1+ (1,0) [0|1] "" RPI + SG_ Roll : 1|12@1+ (0.1,-180) [-180|180] "deg" RPI + SG_ RollAccuracy : 13|9@1+ (0.1,0) [0|50] "deg" RPI + SG_ Pitch : 22|12@1+ (0.1,-90) [-90|90] "deg" RPI + SG_ PitchAccuracy : 34|9@1+ (0.1,0) [0|50] "deg" RPI + SG_ Heading : 43|12@1+ (0.1,0) [0|360] "deg" RPI + SG_ HeadingAccuracy : 55|9@1+ (0.1,0) [0|50] "deg" RPI + +BO_ 774 GnssOdo: 8 GPS + SG_ DistanceValid : 0|1@1+ (1,0) [0|1] "" RPI + SG_ DistanceTrip : 1|22@1+ (1,0) [0|4194303] "m" RPI + SG_ DistanceAccuracy : 23|19@1+ (1,0) [0|524287] "m" RPI + SG_ DistanceTotal : 42|22@1+ (1,0) [0|4194303] "km" RPI + +BO_ 775 GnssSpeed: 5 GPS + SG_ SpeedValid : 0|1@1+ (1,0) [0|1] "" RPI + SG_ Speed : 1|20@1+ (0.001,0) [0|1048.575] "m/s" RPI + SG_ SpeedAccuracy : 21|19@1+ (0.001,0) [0|524.287] "m/s" RPI + +BO_ 776 GnssGeofence: 2 GPS + SG_ FenceValid : 0|1@1+ (1,0) [0|1] "" RPI + SG_ FenceCombined : 1|2@1+ (1,0) [0|1] "" RPI + SG_ Fence1 : 8|2@1+ (1,0) [0|1] "" RPI + SG_ Fence2 : 10|2@1+ (1,0) [0|1] "" RPI + SG_ Fence3 : 12|2@1+ (1,0) [0|1] "" RPI + SG_ Fence4 : 14|2@1+ (1,0) [0|1] "" RPI + +BO_ 777 GnssImu: 8 GPS + SG_ ImuValid : 0|1@1+ (1,0) [0|1] "" RPI + SG_ AccelerationX : 1|10@1+ (0.125,-64) [-64|63.875] "m/s^2" RPI + SG_ AccelerationY : 11|10@1+ (0.125,-64) [-64|63.875] "m/s^2" RPI + SG_ AccelerationZ : 21|10@1+ (0.125,-64) [-64|63.875] "m/s^2" RPI + SG_ AngularRateX : 31|11@1+ (0.25,-256) [-256|255.75] "deg/s" RPI + SG_ AngularRateY : 42|11@1+ (0.25,-256) [-256|255.75] "deg/s" RPI + SG_ AngularRateZ : 53|11@1+ (0.25,-256) [-256|255.75] "deg/s" RPI + +CM_ BO_ 1572 "This ID Transmits at 8 ms."; +CM_ BO_ 1571 "This ID Transmits at 8 ms."; +CM_ BO_ 1573 "This ID Transmits at 8 ms."; +CM_ BO_ 1574 "This ID Transmits at 8 ms."; +CM_ BO_ 2553934720 "Thermistor Module - BMS Broadcast"; +CM_ SG_ 2553934720 ThermModuleNum "Thermistor Module Number"; +CM_ BO_ 2566844926 "Thermistor General Broadcast"; +CM_ SG_ 2566844926 RelThermID "Thermistor ID relative to all configured Thermistor Modules"; +CM_ BO_ 769 "GNSS information"; +CM_ SG_ 769 FixType "Fix type"; +CM_ SG_ 769 Satellites "Number of satellites used"; +CM_ BO_ 770 "GNSS time"; +CM_ SG_ 770 TimeValid "Time validity"; +CM_ SG_ 770 TimeConfirmed "Time confirmed"; +CM_ SG_ 770 Epoch "Epoch time"; +CM_ BO_ 771 "GNSS position"; +CM_ SG_ 771 PositionValid "Position validity"; +CM_ SG_ 771 Latitude "Latitude"; +CM_ SG_ 771 Longitude "Longitude"; +CM_ SG_ 771 PositionAccuracy "Accuracy of position"; +CM_ BO_ 772 "GNSS altitude"; +CM_ SG_ 772 AltitudeValid "Altitude validity"; +CM_ SG_ 772 Altitude "Altitude"; +CM_ SG_ 772 AltitudeAccuracy "Accuracy of altitude"; +CM_ BO_ 773 "GNSS attitude"; +CM_ SG_ 773 AttitudeValid "Attitude validity"; +CM_ SG_ 773 Roll "Vehicle roll"; +CM_ SG_ 773 RollAccuracy "Vehicle roll accuracy"; +CM_ SG_ 773 Pitch "Vehicle pitch"; +CM_ SG_ 773 PitchAccuracy "Vehicle pitch accuracy"; +CM_ SG_ 773 Heading "Vehicle heading"; +CM_ SG_ 773 HeadingAccuracy "Vehicle heading accuracy"; +CM_ BO_ 774 "GNSS odometer"; +CM_ SG_ 774 DistanceTrip "Distance traveled since last reset"; +CM_ SG_ 774 DistanceAccuracy "Distance accuracy (1-sigma)"; +CM_ SG_ 774 DistanceTotal "Distance traveled in total"; +CM_ BO_ 775 "GNSS speed"; +CM_ SG_ 775 SpeedValid "Speed valid"; +CM_ SG_ 775 Speed "Speed m/s"; +CM_ SG_ 775 SpeedAccuracy "Speed accuracy"; +CM_ BO_ 776 "GNSS geofence(s)"; +CM_ SG_ 776 FenceValid "Geofencing status"; +CM_ SG_ 776 FenceCombined "Combined (logical OR) state of all geofences"; +CM_ SG_ 776 Fence1 "Geofence 1 state"; +CM_ SG_ 776 Fence2 "Geofence 2 state"; +CM_ SG_ 776 Fence3 "Geofence 3 state"; +CM_ SG_ 776 Fence4 "Geofence 4 state"; +CM_ BO_ 777 "GNSS IMU"; +CM_ SG_ 777 AccelerationX "X acceleration with a resolution of 0.125 m/s^2"; +CM_ SG_ 777 AccelerationY "Y acceleration with a resolution of 0.125 m/s^2"; +CM_ SG_ 777 AccelerationZ "Z acceleration with a resolution of 0.125 m/s^2"; +CM_ SG_ 777 AngularRateX "X angular rate with a resolution of 0.25 deg/s"; +CM_ SG_ 777 AngularRateY "Y angular rate with a resolution of 0.25 deg/s"; +CM_ SG_ 777 AngularRateZ "Z angular rate with a resolution of 0.25 deg/s"; +BA_DEF_ "BusType" STRING ; +BA_DEF_ "MultiplexExtEnabled" ENUM "No","Yes"; +BA_DEF_DEF_ "BusType" "CAN"; +BA_DEF_DEF_ "MultiplexExtEnabled" "No"; \ No newline at end of file diff --git a/scripts/daq/go.mod b/scripts/daq/go.mod new file mode 100644 index 000000000..395da0651 --- /dev/null +++ b/scripts/daq/go.mod @@ -0,0 +1,28 @@ +module mac-daq + +go 1.24.0 + +require ( + github.com/macformula/hil v0.0.0-20250916142256-e7edb0f50362 + github.com/mattn/go-sqlite3 v1.14.32 + go.einride.tech/can v0.16.1 + go.uber.org/zap v1.26.0 +) + +require ( + github.com/dustin/go-humanize v1.0.1 // indirect + github.com/google/uuid v1.6.0 // indirect + github.com/mattn/go-isatty v0.0.20 // indirect + github.com/ncruces/go-strftime v0.1.9 // indirect + github.com/pkg/errors v0.9.1 // indirect + github.com/remyoudompheng/bigfft v0.0.0-20230129092748-24d4a6f8daec // indirect + go.uber.org/multierr v1.10.0 // indirect + golang.org/x/exp v0.0.0-20250620022241-b7579e27df2b // indirect + golang.org/x/net v0.38.0 // indirect + golang.org/x/sync v0.16.0 // indirect + golang.org/x/sys v0.36.0 // indirect + modernc.org/libc v1.66.10 // indirect + modernc.org/mathutil v1.7.1 // indirect + modernc.org/memory v1.11.0 // indirect + modernc.org/sqlite v1.39.1 // indirect +) diff --git a/scripts/daq/go.sum b/scripts/daq/go.sum new file mode 100644 index 000000000..194882953 --- /dev/null +++ b/scripts/daq/go.sum @@ -0,0 +1,59 @@ +github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c= +github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= +github.com/dustin/go-humanize v1.0.1 h1:GzkhY7T5VNhEkwH0PVJgjz+fX1rhBrR7pRT3mDkpeCY= +github.com/dustin/go-humanize v1.0.1/go.mod h1:Mu1zIs6XwVuF/gI1OepvI0qD18qycQx+mFykh5fBlto= +github.com/golang/mock v1.6.0 h1:ErTB+efbowRARo13NNdxyJji2egdxLGQhRaY+DUumQc= +github.com/golang/mock v1.6.0/go.mod h1:p6yTPP+5HYm5mzsMV8JkE6ZKdX+/wYM6Hr+LicevLPs= +github.com/google/go-cmp v0.6.0 h1:ofyhxvXcZhMsU5ulbFiLKl/XBFqE1GSq7atu8tAmTRI= +github.com/google/go-cmp v0.6.0/go.mod h1:17dUlkBOakJ0+DkrSSNjCkIjxS6bF9zb3elmeNGIjoY= +github.com/google/uuid v1.6.0 h1:NIvaJDMOsjHA8n1jAhLSgzrAzy1Hgr+hNrb57e+94F0= +github.com/google/uuid v1.6.0/go.mod h1:TIyPZe4MgqvfeYDBFedMoGGpEw/LqOeaOT+nhxU+yHo= +github.com/macformula/hil v0.0.0-20250916142256-e7edb0f50362 h1:0ys7bStJcXBrn4hnf+Gr1JcnZgkwSeld2kKCpFD/Sjw= +github.com/macformula/hil v0.0.0-20250916142256-e7edb0f50362/go.mod h1:DZLE3YYqW7GatL2Hh80i04w/X1fQuKhkLoAy6o8a2Uw= +github.com/mattn/go-isatty v0.0.20 h1:xfD0iDuEKnDkl03q4limB+vH+GxLEtL/jb4xVJSWWEY= +github.com/mattn/go-isatty v0.0.20/go.mod h1:W+V8PltTTMOvKvAeJH7IuucS94S2C6jfK/D7dTCTo3Y= +github.com/mattn/go-sqlite3 v1.14.32 h1:JD12Ag3oLy1zQA+BNn74xRgaBbdhbNIDYvQUEuuErjs= +github.com/mattn/go-sqlite3 v1.14.32/go.mod h1:Uh1q+B4BYcTPb+yiD3kU8Ct7aC0hY9fxUwlHK0RXw+Y= +github.com/ncruces/go-strftime v0.1.9 h1:bY0MQC28UADQmHmaF5dgpLmImcShSi2kHU9XLdhx/f4= +github.com/ncruces/go-strftime v0.1.9/go.mod h1:Fwc5htZGVVkseilnfgOVb9mKy6w1naJmn9CehxcKcls= +github.com/pkg/errors v0.9.1 h1:FEBLx1zS214owpjy7qsBeixbURkuhQAwrK5UwLGTwt4= +github.com/pkg/errors v0.9.1/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0= +github.com/pmezard/go-difflib v1.0.0 h1:4DBwDE0NGyQoBHbLQYPwSUPoCMWR5BEzIk/f1lZbAQM= +github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= +github.com/remyoudompheng/bigfft v0.0.0-20230129092748-24d4a6f8daec h1:W09IVJc94icq4NjY3clb7Lk8O1qJ8BdBEF8z0ibU0rE= +github.com/remyoudompheng/bigfft v0.0.0-20230129092748-24d4a6f8daec/go.mod h1:qqbHyh8v60DhA7CoWK5oRCqLrMHRGoxYCSS9EjAz6Eo= +github.com/stretchr/testify v1.8.4 h1:CcVxjf3Q8PM0mHUKJCdn+eZZtm5yQwehR5yeSVQQcUk= +github.com/stretchr/testify v1.8.4/go.mod h1:sz/lmYIOXD/1dqDmKjjqLyZ2RngseejIcXlSw2iwfAo= +go.einride.tech/can v0.16.1 h1:s9MqX1OR6ujGxvl+gOWAGL54MC3kaPE+cgxBCUfDrB8= +go.einride.tech/can v0.16.1/go.mod h1:9pgqXNGpPfrd/WGXGmiKW8cUvIep/o+o76JgUKpQuWI= +go.uber.org/goleak v1.3.0 h1:2K3zAYmnTNqV73imy9J1T3WC+gmCePx2hEGkimedGto= +go.uber.org/goleak v1.3.0/go.mod h1:CoHD4mav9JJNrW/WLlf7HGZPjdw8EucARQHekz1X6bE= +go.uber.org/multierr v1.10.0 h1:S0h4aNzvfcFsC3dRF1jLoaov7oRaKqRGC/pUEJ2yvPQ= +go.uber.org/multierr v1.10.0/go.mod h1:20+QtiLqy0Nd6FdQB9TLXag12DsQkrbs3htMFfDN80Y= +go.uber.org/zap v1.26.0 h1:sI7k6L95XOKS281NhVKOFCUNIvv9e0w4BF8N3u+tCRo= +go.uber.org/zap v1.26.0/go.mod h1:dtElttAiwGvoJ/vj4IwHBS/gXsEu/pZ50mUIRWuG0so= +golang.org/x/exp v0.0.0-20250620022241-b7579e27df2b h1:M2rDM6z3Fhozi9O7NWsxAkg/yqS/lQJ6PmkyIV3YP+o= +golang.org/x/exp v0.0.0-20250620022241-b7579e27df2b/go.mod h1:3//PLf8L/X+8b4vuAfHzxeRUl04Adcb341+IGKfnqS8= +golang.org/x/net v0.38.0 h1:vRMAPTMaeGqVhG5QyLJHqNDwecKTomGeqbnfZyKlBI8= +golang.org/x/net v0.38.0/go.mod h1:ivrbrMbzFq5J41QOQh0siUuly180yBYtLp+CKbEaFx8= +golang.org/x/sync v0.12.0 h1:MHc5BpPuC30uJk597Ri8TV3CNZcTLu6B6z4lJy+g6Jw= +golang.org/x/sync v0.12.0/go.mod h1:1dzgHSNfp02xaA81J2MS99Qcpr2w7fw1gpm99rleRqA= +golang.org/x/sync v0.16.0 h1:ycBJEhp9p4vXvUZNszeOq0kGTPghopOL8q0fq3vstxw= +golang.org/x/sync v0.16.0/go.mod h1:1dzgHSNfp02xaA81J2MS99Qcpr2w7fw1gpm99rleRqA= +golang.org/x/sys v0.6.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= +golang.org/x/sys v0.31.0 h1:ioabZlmFYtWhL+TRYpcnNlLwhyxaM9kWTDEmfnprqik= +golang.org/x/sys v0.31.0/go.mod h1:BJP2sWEmIv4KK5OTEluFJCKSidICx8ciO85XgH3Ak8k= +golang.org/x/sys v0.36.0 h1:KVRy2GtZBrk1cBYA7MKu5bEZFxQk4NIDV6RLVcC8o0k= +golang.org/x/sys v0.36.0/go.mod h1:OgkHotnGiDImocRcuBABYBEXf8A9a87e/uXjp9XT3ks= +gopkg.in/yaml.v3 v3.0.1 h1:fxVm/GzAzEWqLHuvctI91KS9hhNmmWOoWu0XTYJS7CA= +gopkg.in/yaml.v3 v3.0.1/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM= +gotest.tools/v3 v3.5.1 h1:EENdUnS3pdur5nybKYIh2Vfgc8IUNBjxDPSjtiJcOzU= +gotest.tools/v3 v3.5.1/go.mod h1:isy3WKz7GK6uNw/sbHzfKBLvlvXwUyV06n6brMxxopU= +modernc.org/libc v1.66.10 h1:yZkb3YeLx4oynyR+iUsXsybsX4Ubx7MQlSYEw4yj59A= +modernc.org/libc v1.66.10/go.mod h1:8vGSEwvoUoltr4dlywvHqjtAqHBaw0j1jI7iFBTAr2I= +modernc.org/mathutil v1.7.1 h1:GCZVGXdaN8gTqB1Mf/usp1Y/hSqgI2vAGGP4jZMCxOU= +modernc.org/mathutil v1.7.1/go.mod h1:4p5IwJITfppl0G4sUEDtCr4DthTaT47/N3aT6MhfgJg= +modernc.org/memory v1.11.0 h1:o4QC8aMQzmcwCK3t3Ux/ZHmwFPzE6hf2Y5LbkRs+hbI= +modernc.org/memory v1.11.0/go.mod h1:/JP4VbVC+K5sU2wZi9bHoq2MAkCnrt2r98UGeSK7Mjw= +modernc.org/sqlite v1.39.1 h1:H+/wGFzuSCIEVCvXYVHX5RQglwhMOvtHSv+VtidL2r4= +modernc.org/sqlite v1.39.1/go.mod h1:9fjQZ0mB1LLP0GYrp39oOJXx/I2sxEnZtzCmEQIKvGE= diff --git a/scripts/daq/heartbeat.go b/scripts/daq/heartbeat.go new file mode 100644 index 000000000..3e9629d71 --- /dev/null +++ b/scripts/daq/heartbeat.go @@ -0,0 +1,94 @@ +package main + +import ( + "bytes" + "crypto/rand" + "encoding/hex" + "encoding/json" + "fmt" + "net" + "net/http" + "os" + "time" + + "go.uber.org/zap" +) + +type HeartbeatHandler struct { + serverURL string + vehicleID string + sessionID string + can0 net.Conn + can1 net.Conn + logger *zap.Logger +} + +func NewHeartbeatHandler(can0, can1 net.Conn, logger *zap.Logger) *HeartbeatHandler { + + // Configuring Vehicle ID + vehicleID := os.Getenv("VEHICLE_ID") + if vehicleID == "" { + logger.Error("VEHICLE_ID not found, using default.") + vehicleID = "default" + } + + // Generating Session ID + sessionBytes := make([]byte, 16) + _, err := rand.Read(sessionBytes) + var sessionID string + if err != nil { + logger.Warn("Failed to generate session ID, defaulting to time based session ID.") + sessionID = fmt.Sprintf("fallback-%d", time.Now().UnixNano()) + } else { + sessionID = hex.EncodeToString(sessionBytes) + } + + serverURL := os.Getenv("SERVER_URL") + if serverURL == "" { + logger.Error("SERVER_URL for heartbeatnot found") + } + + return &HeartbeatHandler{ + serverURL: serverURL, + vehicleID: vehicleID, + sessionID: sessionID, + can0: can0, + can1: can1, + logger: logger, + } +} + +func (h *HeartbeatHandler) SendHeartbeat() error { + can0Active, can1Active := h.checkCAN() + + payload := map[string]interface{}{ + "timestamp": time.Now().UnixMilli(), + "vehicle_id": h.vehicleID, + "session_id": h.sessionID, + "can_status": map[string]bool{ + "can0": can0Active, + "can1": can1Active, + }, + } + + jsonPayload, err := json.Marshal(payload) + if err != nil { + h.logger.Error("Failed to convert heartbeat payload to JSON", zap.Error(err)) + return err + } + + response, err := http.Post(h.serverURL, "application/json", bytes.NewBuffer(jsonPayload)) + if err != nil { + h.logger.Error("Failed to send heartbeat", zap.Error(err)) + return err + } + defer response.Body.Close() + + return nil +} + +func (h *HeartbeatHandler) checkCAN() (bool, bool) { + can0Active := h.can0 != nil + can1Active := h.can1 != nil + return can0Active, can1Active +} diff --git a/scripts/daq/logger.py b/scripts/daq/logger.py new file mode 100644 index 000000000..21686ceb6 --- /dev/null +++ b/scripts/daq/logger.py @@ -0,0 +1,285 @@ +#!/usr/bin/env python3 +""" +Raspberry Pi CAN Data Logger. + +Reads CAN messages from one or two SocketCAN interfaces via candump, decodes +them using the vehicle and powertrain DBC files, and writes decoded signals +to a timestamped CSV file. + +Usage: + python logger.py [interface] [--pt-interface PT_INTERFACE] + python logger.py vcan0 + python logger.py can0 --pt-interface can1 +""" + +import argparse +import queue +import re +import signal +import subprocess +import sys +import threading +from datetime import datetime +from pathlib import Path + +import cantools + + +# --------------------------------------------------------------------------- +# Target message IDs — what we care about logging. +# +# How these IDs work: +# Standard CAN frames use 11-bit IDs (0x000–0x7FF). +# Extended CAN frames use 29-bit IDs. +# +# In the DBC file, extended messages are written with bit 31 set, e.g.: +# BO_ 2553934720 BmsBroadcast ... +# (2553934720 = 0x9839F380, bit 31 set) +# +# cantools strips bit 31 when it parses the DBC and stores the actual +# 29-bit frame ID: +# BmsBroadcast.frame_id == 0x1839F380 == 406451072 +# +# candump prints extended IDs as 8 hex digits without bit 31: +# (t) vcan0 1839F380#... +# +# So the IDs here must match cantools (29-bit, no bit-31 flag). +# --------------------------------------------------------------------------- +TARGET_IDS = { + 230, # DashCommand (veh) - Speed + 300, # Accumulator_Soc (veh) - battery state + 310, # SuspensionTravel34 (veh) - linear pots + 401, # AppsDebug (veh) - accelerator pedal pots + 402, # BppsSteerDebug (veh) - brake pedal + steering pots + 643, # Inv1_ActualValues1 (pt) - left motor velocity + 644, # Inv2_ActualValues1 (pt) - right motor velocity + 645, # Inv1_ActualValues2 (pt) - motor/inverter temps + 646, # Inv2_ActualValues2 (pt) - motor/inverter temps + 773, # GnssAttitude (veh) - IMU roll/pitch/heading + 777, # GnssImu (veh) - IMU accelerations + rates + 1572, # Pack_State (veh) - battery current/voltage + 1573, # Pack_SOC (veh) - battery state of charge + 406451072, # BmsBroadcast (veh) - battery pack temps (extended ID 0x1839F380) + 419361278, # ThermistorBroadcast (veh) - thermistor temps (extended ID 0x18FEF1FE) +} + +# candump -L log format: (timestamp) interface hex_id#hex_data +CANDUMP_LINE_RE = re.compile( + r'\((\d+\.\d+)\)\s+\S+\s+([0-9A-Fa-f]+)#([0-9A-Fa-f]*)' +) + + +def load_dbc(veh_dbc: Path, pt_dbc: Path | None) -> cantools.database.Database: + """Load veh.dbc and optionally pt.dbc into a single cantools Database.""" + db = cantools.database.load_file(str(veh_dbc)) + if pt_dbc and pt_dbc.exists(): + with open(pt_dbc, encoding='utf-8') as f: + db.add_dbc(f) + return db + + +def open_csv(log_dir: Path) -> tuple[Path, object]: + """Create logs dir, open CSV with header, return (path, file).""" + log_dir.mkdir(parents=True, exist_ok=True) + ts = datetime.now().strftime('%Y%m%d_%H%M%S') + path = log_dir / f'can_log_{ts}.csv' + f = open(path, 'w', newline='', encoding='utf-8') + f.write('timestamp_epoch_s,message_id_hex,message_name,signal_name,value,unit\n') + return path, f + + +def parse_candump_line(line: str) -> tuple[float, int, bytes] | None: + """ + Parse a candump -L line into (timestamp_s, can_id, data_bytes). + + candump output format: + (1709650000.123456) vcan0 136#8E91 + (1709650000.456789) vcan0 1839F380#C2001816060A0041 + + The hex ID is printed as-is by candump — no bit-31 flag, just the raw + value. For extended frames this is 8 hex digits; for standard frames + it is up to 3 hex digits. We parse it directly so it matches cantools. + """ + m = CANDUMP_LINE_RE.match(line.strip()) + if not m: + return None + ts_str, hex_id, hex_data = m.groups() + if len(hex_data) % 2 != 0: + return None + return float(ts_str), int(hex_id, 16), bytes.fromhex(hex_data) + + +def decode_frame( + db: cantools.database.Database, + can_id: int, + data: bytes, +) -> tuple[str, dict[str, tuple]] | None: + """ + Look up the message in the DBC by frame ID and decode the raw bytes. + + Returns (message_name, {signal_name: (value, unit)}) or None if the + message is not in the DBC or decoding fails. + """ + msg = next((m for m in db.messages if m.frame_id == can_id), None) + if msg is None: + return None + try: + decoded = msg.decode(data) + except Exception: + return None + out = { + sig.name: (decoded[sig.name], sig.unit or '') + for sig in msg.signals + if sig.name in decoded and decoded[sig.name] is not None + } + return (msg.name, out) if out else None + + +def _candump_reader( + interface: str, + out_queue: queue.Queue, + processes: list, +) -> None: + """ + Spawn candump on `interface`, forward each line to out_queue. + Puts a None sentinel when done so the consumer knows one source finished. + """ + try: + proc = subprocess.Popen( + ['candump', '-L', interface], + stdout=subprocess.PIPE, + text=True, + bufsize=1, + ) + except FileNotFoundError: + print( + "Error: 'candump' not found.\n" + "On the Raspberry Pi install can-utils: sudo apt install can-utils", + file=sys.stderr, + ) + out_queue.put(None) + return + + processes.append(proc) + for line in proc.stdout: + out_queue.put(line) + out_queue.put(None) + + +def main() -> int: + parser = argparse.ArgumentParser( + description='CAN data logger: candump -> DBC decode -> CSV' + ) + parser.add_argument( + 'interface', + nargs='?', + default='vcan0', + help='Primary SocketCAN interface (default: vcan0)', + ) + parser.add_argument( + '--pt-interface', + default=None, + metavar='INTERFACE', + help='Optional second interface for powertrain bus (e.g. can1)', + ) + parser.add_argument( + '--veh-dbc', + type=Path, + default=None, + help='Path to veh.dbc (default: /dbc/veh.dbc)', + ) + parser.add_argument( + '--pt-dbc', + type=Path, + default=None, + help='Path to pt.dbc (default: /projects/pt.dbc)', + ) + parser.add_argument( + '--log-dir', + type=Path, + default=None, + help='Output directory for CSV logs (default: /logs)', + ) + parser.add_argument( + '--all', + action='store_true', + help='Log every decodable message, not just target signals', + ) + args = parser.parse_args() + + script_dir = Path(__file__).resolve().parent + veh_dbc = args.veh_dbc or (script_dir / 'dbc' / 'veh.dbc') + pt_dbc = args.pt_dbc or (script_dir.parent.parent / 'projects' / 'pt.dbc') + log_dir = args.log_dir or (script_dir / 'logs') + + if not veh_dbc.exists(): + print(f'Error: veh.dbc not found at {veh_dbc}', file=sys.stderr) + return 1 + + db = load_dbc(veh_dbc, pt_dbc) + log_path, csv_file = open_csv(log_dir) + + interfaces = [args.interface] + if args.pt_interface: + interfaces.append(args.pt_interface) + + print(f'Logging to {log_path}', file=sys.stderr) + print(f'Interfaces: {", ".join(interfaces)}. Press Ctrl+C to stop.', file=sys.stderr) + + # Use a queue so frames from multiple candump processes merge into one loop. + line_queue: queue.Queue = queue.Queue() + processes: list = [] + + for iface in interfaces: + t = threading.Thread( + target=_candump_reader, + args=(iface, line_queue, processes), + daemon=True, + ) + t.start() + + def cleanup(_sig=None, _frame=None): + for p in processes: + p.terminate() + p.wait() + csv_file.close() + sys.exit(0) + + signal.signal(signal.SIGINT, cleanup) + signal.signal(signal.SIGTERM, cleanup) + + # Count how many None sentinels we expect (one per interface thread). + num_interfaces = len(interfaces) + done = 0 + + while done < num_interfaces: + line = line_queue.get() + if line is None: + done += 1 + continue + + parsed = parse_candump_line(line) + if not parsed: + continue + timestamp, can_id, data = parsed + + if not args.all and can_id not in TARGET_IDS: + continue + + result = decode_frame(db, can_id, data) + if not result: + continue + + msg_name, signals = result + id_hex = f'0x{can_id:X}' + for sig_name, (value, unit) in signals.items(): + csv_file.write( + f'{timestamp},{id_hex},{msg_name},{sig_name},{value},"{unit}"\n' + ) + + csv_file.close() + return 0 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/scripts/daq/main.go b/scripts/daq/main.go new file mode 100644 index 000000000..49fc945d1 --- /dev/null +++ b/scripts/daq/main.go @@ -0,0 +1,64 @@ +package main + +import ( + "context" + "fmt" + vehcan "mac-daq/generated" + "time" + + "github.com/macformula/hil/canlink" + "go.einride.tech/can/pkg/socketcan" + "go.uber.org/zap" +) + +func main() { + can0, err := socketcan.DialContext(context.Background(), "can", "can0") + if err != nil { + panic(err) + } + + can1, err := socketcan.DialContext(context.Background(), "can", "can1") + if err != nil { + panic(err) + } + + logger, _ := zap.NewDevelopment() + + heartbeat := NewHeartbeatHandler(can0, can1, logger) + + telemetry, err := NewTelemetryHandler("./can_cache.sqlite") + if err != nil { + panic(err) + } + + daqCan0 := NewDaqHandler(vehcan.Messages(), telemetry, "can0", logger) + daqCan1 := NewDaqHandler(vehcan.Messages(), telemetry, "can1", logger) + + manager0 := canlink.NewBusManager(logger, &can0) + manager0.Register(daqCan0) + manager0.Start(context.Background()) + + manager1 := canlink.NewBusManager(logger, &can1) + manager1.Register(daqCan1) + manager1.Start(context.Background()) + + uploadTimer := time.NewTimer(time.Second) + + heartbeatInterval := time.NewTicker(3 * time.Second) + defer heartbeatInterval.Stop() + + for { + select { + case <-uploadTimer.C: + err = telemetry.Upload() + if err != nil { + fmt.Printf("failed to upload telemetry data: %v\n", err) + } + case <-heartbeatInterval.C: + err = heartbeat.SendHeartbeat() + if err != nil { + logger.Error("Failed to send heartbeat", zap.Error(err)) + } + } + } +} diff --git a/scripts/daq/requirements.txt b/scripts/daq/requirements.txt new file mode 100644 index 000000000..c76b518b5 --- /dev/null +++ b/scripts/daq/requirements.txt @@ -0,0 +1 @@ +cantools>=39.0.0 diff --git a/scripts/daq/telemetry.go b/scripts/daq/telemetry.go new file mode 100644 index 000000000..e7754ffc8 --- /dev/null +++ b/scripts/daq/telemetry.go @@ -0,0 +1,201 @@ +package main + +import ( + "container/list" + "database/sql" + "fmt" + + "github.com/macformula/hil/canlink" + "go.einride.tech/can" + "go.uber.org/zap" + _ "modernc.org/sqlite" +) + +const BufferSize = 512 + +type TelemetryPacket struct { + Id int64 `db:"id"` + Timestamp int64 `db:"timestamp"` + FrameId uint32 `db:"frame_id"` + FrameData can.Data `db:"frame_data"` +} + +type TelemetryHandler struct { + buf *list.List + db *sql.DB + l *zap.Logger + dbFile string +} + +func NewTelemetryHandler(dbFile string) (*TelemetryHandler, error) { + db, err := sql.Open("sqlite", dbFile) + if err != nil { + return nil, err + } + + _, err = db.Exec(` + CREATE TABLE IF NOT EXISTS can_cache ( + id INTEGER PRIMARY KEY, + timestamp INTEGER, + frame_id INTEGER, + frame_data INTEGER, + bus_name TEXT + ) + `) + if err != nil { + db.Close() + return nil, err + } + + _, err = db.Exec("CREATE INDEX IF NOT EXISTS idx_timestamp ON can_cache(timestamp)") + if err != nil { + db.Close() + return nil, err + } + + t := TelemetryHandler{ + buf: list.New(), + db: db, + } + + err = t.fillBufferFromDisk() + if err != nil { + db.Close() + return nil, err + } + + fmt.Printf("Initial buffer size: %v\n", t.buf.Len()) + + return &t, nil +} + +func (t *TelemetryHandler) Enqueue(frame canlink.TimestampedFrame, busName string) error { + query, err := t.db.Prepare(` + INSERT INTO can_cache (timestamp, frame_id, frame_data, bus_name) VALUES (?, ?, ?, ?) + `) + if err != nil { + return err + } + + res, err := query.Exec(frame.Time.UnixMilli(), frame.Frame.ID, frame.Frame.Data.PackBigEndian(), busName) + if err != nil { + return err + } + + // Ignore the error since the schema enforces an auto-incrementing primary key + id, _ := res.LastInsertId() + + elem := list.Element{ + Value: TelemetryPacket{ + Id: id, + Timestamp: frame.Time.UnixMilli(), + FrameId: frame.Frame.ID, + FrameData: frame.Frame.Data, + }, + } + + if t.buf.Len() >= BufferSize { + // go upload ? + t.buf.Remove(t.buf.Back()) + } + t.buf.MoveToFront(&elem) + + return nil +} + +func (t *TelemetryHandler) Upload() error { + // pop from front of buffer and write to backend + + // Ignore for now ... + /*buf := t.emptyBuffer() + + type temp struct { + Value []byte `json:"value"` + Timestamp int64 `json:"timestamp"` + } + + for i := range len(buf) { + frame := can.Frame{ + ID: buf[i].FrameId, + Data: buf[i].FrameData, + Length: 8, + } + rawFrame, _ := json.Marshal(frame) + + toWrite := temp{ + Timestamp: buf[i].Timestamp, + Value: rawFrame, + } + + data, err := json.Marshal(toWrite) + if err != nil { + return err + } + if i == 0 { + fmt.Printf("payload: %s\n", string(data)) + } + + // If this fails then we should re-queue the entire buffer to not lose anything + _, err = http.Post("http://localhost:5000/write-graph", "application/json", bytes.NewBuffer(data)) + if err != nil { + fmt.Printf("telemetry/upload: failed to post frame data: %v\n", err) + return err + } + } + + // In case of an error here, just log and keep going, we still need to upload our data + err := t.fillBufferFromDisk() + if err != nil { + fmt.Printf("telemetry/upload: failed to fill buffer: %v\n", err) + }*/ + + return nil +} + +func (t *TelemetryHandler) emptyBuffer() []TelemetryPacket { + packets := make([]TelemetryPacket, t.buf.Len()) + next := t.buf.Front() + + for range t.buf.Len() { + packets = append(packets, next.Value.(TelemetryPacket)) + next = next.Next() + } + + return packets +} + +func (t *TelemetryHandler) fillBufferFromDisk() error { + t.buf.Init() + + query, err := t.db.Prepare(` + SELECT id, timestamp, frame_id, frame_data FROM can_cache ORDER BY timestamp DESC LIMIT ? + `) + if err != nil { + return err + } + + rows, err := query.Query(BufferSize) + if err != nil { + return err + } + + for rows.Next() { + var packet TelemetryPacket + var packedFrameData uint64 + + err = rows.Scan(&packet.Id, &packet.Id, &packet.FrameId, &packedFrameData) + if err != nil { + fmt.Printf("Failed to scan packet: %v\n", err) + continue + } + + var frameData can.Data + frameData.UnpackBigEndian(packedFrameData) + packet.FrameData = frameData + + t.buf.PushBack(packet) + } + + rows.Close() + return nil +}