From cc177ef1b55ef077a404bc442a67d3bdc99a10cb Mon Sep 17 00:00:00 2001 From: Andrew Zhang Date: Tue, 26 Nov 2024 15:05:56 -0500 Subject: [PATCH] janky testing --- MCU.code-workspace | 15 + platformio.ini | 3 +- src/ethtest.cpp | 139 ++++++++ src/main.cpp | 679 ------------------------------------- src/main_ethernet_test.cpp | 42 --- src/main_spi_test.cpp | 25 -- src/test_can_interface.cpp | 137 -------- 7 files changed, 156 insertions(+), 884 deletions(-) create mode 100644 MCU.code-workspace create mode 100644 src/ethtest.cpp delete mode 100644 src/main.cpp delete mode 100644 src/main_ethernet_test.cpp delete mode 100644 src/main_spi_test.cpp delete mode 100644 src/test_can_interface.cpp diff --git a/MCU.code-workspace b/MCU.code-workspace new file mode 100644 index 000000000..44506e4e6 --- /dev/null +++ b/MCU.code-workspace @@ -0,0 +1,15 @@ +{ + "folders": [ + { + "path": "." + }, + { + "path": "../code-2024/Libraries/QNEthernet" + } + ], + "settings": { + "files.associations": { + "algorithm": "cpp" + } + } +} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 5ef95794c..0b563204f 100644 --- a/platformio.ini +++ b/platformio.ini @@ -38,12 +38,13 @@ lib_ignore = ; refactoring build_src_filter = -<**/*.cpp> - + + + build_unflags = -std=gnu++11 build_flags = -std=c++17 -D TEENSY_OPT_SMALLEST_CODE ; -D ENABLE_DEBUG_PRINTS + -DQNETHERNET_ENABLE_RAW_FRAME_SUPPORT=1 check_tool = clangtidy check_src_filters = + diff --git a/src/ethtest.cpp b/src/ethtest.cpp new file mode 100644 index 000000000..93259e33f --- /dev/null +++ b/src/ethtest.cpp @@ -0,0 +1,139 @@ +#include +// SPDX-FileCopyrightText: (c) 2022-2024 Shawn Silverman +// SPDX-License-Identifier: AGPL-3.0-or-later + +// RawFrameMonitor prints all unknown raw Ethernet frames. Known frame +// types won't be printed if the associated features are enabled and +// there's a handler for them. These may include: +// 1. IPv4 (0x0800) +// 2. ARP (0x0806) +// 3. IPv6 (0x86DD) (if enabled) +// +// Currently, in order to receive frames not addressed to the device's +// MAC address or to a subscribed multicast address, the destination +// address must be tagged as "allowed" by calling +// `Ethernet.setMACAddressAllowed(mac, flag)`. Alternatively, enable +// promiscuous mode by defining the QNETHERNET_ENABLE_PROMISCUOUS_MODE +// macro. +// +// In order to use this example, define the +// QNETHERNET_ENABLE_RAW_FRAME_SUPPORT macro. +// +// Note: the configuration macros must either be defined in the project build +// options or in the qnethernet_opts.h library file. +// +// This file is part of the QNEthernet library. + +// C++ includes +#include + +#include + +using namespace qindesign::network; + +// VLAN EtherType constants +constexpr uint16_t kEtherTypeVLAN = 0x8100u; +constexpr uint16_t kEtherTypeQinQ = 0x88A8u; + +// We can access the frame's internal data buffer directly, +// so we don't need the following: +// // Buffer for reading frames. +// uint8_t buf[EthernetFrame.maxFrameLen()]; + +// Tracks the received frame count. +int frameCount = 0; + +// Main program setup. +void setup() { + Serial.begin(115200); + while (!Serial && millis() < 4000) { + // Wait for Serial + } + printf("Starting...\r\n"); + + // Print the MAC address + uint8_t mac[6]; + Ethernet.macAddress(mac); // This is informative; it retrieves, not sets + printf("MAC = %02x:%02x:%02x:%02x:%02x:%02x\r\n", + mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); + + // Add listeners before starting Ethernet + + Ethernet.onLinkState([](bool state) { + printf("[Ethernet] Link %s\r\n", state ? "ON" : "OFF"); + }); + + Ethernet.onAddressChanged([]() { + IPAddress ip = Ethernet.localIP(); + bool hasIP = (ip != INADDR_NONE); + if (hasIP) { + IPAddress subnet = Ethernet.subnetMask(); + IPAddress broadcast = Ethernet.broadcastIP(); + IPAddress gw = Ethernet.gatewayIP(); + IPAddress dns = Ethernet.dnsServerIP(); + + printf("[Ethernet] Address changed:\r\n" + " Local IP = %u.%u.%u.%u\r\n" + " Subnet = %u.%u.%u.%u\r\n" + " Broadcast IP = %u.%u.%u.%u\r\n" + " Gateway = %u.%u.%u.%u\r\n" + " DNS = %u.%u.%u.%u\r\n", + ip[0], ip[1], ip[2], ip[3], + subnet[0], subnet[1], subnet[2], subnet[3], + broadcast[0], broadcast[1], broadcast[2], broadcast[3], + gw[0], gw[1], gw[2], gw[3], + dns[0], dns[1], dns[2], dns[3]); + } else { + printf("[Ethernet] Address changed: No IP address\r\n"); + } + }); + + // Initialize Ethernet + // Optionally turn DHCP off by uncommenting the following line: + // Ethernet.setDHCPEnabled(false); + printf("Starting Ethernet%s...\r\n", + Ethernet.isDHCPEnabled() ? " with DHCP" : ""); + if (!Ethernet.begin()) { + printf("Failed to start Ethernet\r\n"); + return; + } +} + +// Main program loop. +void loop() { + // Example MAC addresses (replace with arbitrary MAC addresses as needed) + const uint8_t destinationMAC[6] = {0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC}; + const uint8_t sourceMAC[6] = {0xDE, 0xAD, 0xBE, 0xEF, 0x00, 0x01}; + // Example EtherType (e.g., IPv4) + constexpr uint16_t etherType = 0x0800; + + if (Serial.available()) + { + int c = Serial.read(); + if (c == 'b') + { + int starttime = millis(); + //for (size_t i = 0; i < 1000; i++) + while (millis()-starttime < 1000) + { + + EthernetFrame.beginFrame(destinationMAC, sourceMAC, etherType); + EthernetFrame.print("123123123132123123123132123123123132123123123132"); + if (!EthernetFrame.endFrame()) { + break; + } + delayMicroseconds(50); + } + Serial.println("done"); + } + + } + + + // Updated code + //EthernetFrame.beginFrame(destinationMAC, sourceMAC, etherType); + //EthernetFrame.print("123123123132"); + //EthernetFrame.endFrame(); + //delay(1000); + //Serial.println("test"); +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 9cbe128f1..000000000 --- a/src/main.cpp +++ /dev/null @@ -1,679 +0,0 @@ -#ifdef ENABLE_DEBUG_PRINTS -#define DEBUG_PRINTS true -#else -#define DEBUG_PRINTS false -#endif - -/* Include files */ -/* System Includes*/ -#include -/* Libraries */ -#include "FlexCAN_T4.h" -#include "HyTech_CAN.h" -#include "MCU_rev15_defs.h" -#include "pb.h" -// /* Interfaces */ -#include "DrivebrainETHInterface.h" -#include "ProtobufMsgInterface.h" -#include "HytechCANInterface.h" -#include "ThermistorInterface.h" -#include "Teensy_ADC.h" -#include "MCP_ADC.h" -#include "ORBIS_BR10.h" -#include "MCUInterface.h" -#include "AMSInterface.h" -#include "WatchdogInterface.h" -#include "DashboardInterface.h" -#include "InverterInterface.h" -#include "TelemetryInterface.h" -#include "SABInterface.h" -#include "VectornavInterface.h" -#include "LoadCellInterface.h" - -/* Systems */ -#include "SysClock.h" -#include "Buzzer.h" -#include "SafetySystem.h" -#include "DrivetrainSystem.h" -#include "PedalsSystem.h" -#include "DrivebrainController.h" -#include "TorqueControllerMux.h" -#include "TorqueControllers.h" -#include "CASESystem.h" - -// /* State machine */ -#include "MCUStateMachine.h" -#include "HT08_CASE.h" - -#include "InterfaceParams.h" -#include "PrintLogger.h" - -#include "hytech_msgs.pb.h" -/* - PARAMETER STRUCTS -*/ -using namespace qindesign::network; - -const TelemetryInterfaceReadChannels telem_read_channels = { - .accel1_channel = MCU15_ACCEL1_CHANNEL, - .accel2_channel = MCU15_ACCEL2_CHANNEL, - .brake1_channel = MCU15_BRAKE1_CHANNEL, - .brake2_channel = MCU15_BRAKE2_CHANNEL, - .pots_fl_channel = MCU15_FL_POTS_CHANNEL, - .pots_fr_channel = MCU15_FR_POTS_CHANNEL, - .loadcell_fl_channel = MCU15_FL_LOADCELL_CHANNEL, - .loadcell_fr_channel = MCU15_FR_LOADCELL_CHANNEL, - .analog_steering_channel = MCU15_STEERING_CHANNEL, - .current_channel = MCU15_CUR_POS_SENSE_CHANNEL, - .current_ref_channel = MCU15_CUR_NEG_SENSE_CHANNEL, - .glv_sense_channel = MCU15_GLV_SENSE_CHANNEL, - .therm_fl_channel = MCU15_THERM_FL_CHANNEL, - .therm_fr_channel = MCU15_THERM_FR_CHANNEL}; - -const PedalsParams accel1_only_params = { - .min_pedal_1 = ACCEL1_PEDAL_MIN, - .min_pedal_2 = ACCEL1_PEDAL_MIN, - .max_pedal_1 = ACCEL1_PEDAL_MAX, - .max_pedal_2 = ACCEL1_PEDAL_MAX, - .min_sensor_pedal_1 = ACCEL1_PEDAL_OOR_MIN, - .min_sensor_pedal_2 = ACCEL1_PEDAL_OOR_MIN, - .max_sensor_pedal_1 = ACCEL1_PEDAL_OOR_MAX, - .max_sensor_pedal_2 = ACCEL1_PEDAL_OOR_MAX, - .activation_percentage = APPS_ACTIVATION_PERCENTAGE, - .deadzone_margin = DEFAULT_PEDAL_DEADZONE, - .implausibility_margin = DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, - .mechanical_activation_percentage = APPS_ACTIVATION_PERCENTAGE}; - -const PedalsParams accel2_only_params = { - .min_pedal_1 = ACCEL2_PEDAL_MIN, - .min_pedal_2 = ACCEL2_PEDAL_MIN, - .max_pedal_1 = ACCEL2_PEDAL_MAX, - .max_pedal_2 = ACCEL2_PEDAL_MAX, - .min_sensor_pedal_1 = ACCEL2_PEDAL_OOR_MIN, - .min_sensor_pedal_2 = ACCEL2_PEDAL_OOR_MIN, - .max_sensor_pedal_1 = ACCEL2_PEDAL_OOR_MAX, - .max_sensor_pedal_2 = ACCEL2_PEDAL_OOR_MAX, - .activation_percentage = APPS_ACTIVATION_PERCENTAGE, - .deadzone_margin = DEFAULT_PEDAL_DEADZONE, - .implausibility_margin = DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, - .mechanical_activation_percentage = APPS_ACTIVATION_PERCENTAGE}; - -const PedalsParams accel_params = { - .min_pedal_1 = ACCEL1_PEDAL_MIN, - .min_pedal_2 = ACCEL2_PEDAL_MIN, - .max_pedal_1 = ACCEL1_PEDAL_MAX, - .max_pedal_2 = ACCEL2_PEDAL_MAX, - .min_sensor_pedal_1 = ACCEL1_PEDAL_OOR_MIN, - .min_sensor_pedal_2 = ACCEL2_PEDAL_OOR_MIN, - .max_sensor_pedal_1 = ACCEL1_PEDAL_OOR_MAX, - .max_sensor_pedal_2 = ACCEL2_PEDAL_OOR_MAX, - .activation_percentage = APPS_ACTIVATION_PERCENTAGE, - .deadzone_margin = DEFAULT_PEDAL_DEADZONE, - .implausibility_margin = DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, - .mechanical_activation_percentage = APPS_ACTIVATION_PERCENTAGE}; - -const PedalsParams brake1_only_params = { - .min_pedal_1 = BRAKE1_PEDAL_MIN, - .min_pedal_2 = BRAKE1_PEDAL_MIN, - .max_pedal_1 = BRAKE1_PEDAL_MAX, - .max_pedal_2 = BRAKE1_PEDAL_MAX, - .min_sensor_pedal_1 = BRAKE1_PEDAL_OOR_MIN, - .min_sensor_pedal_2 = BRAKE1_PEDAL_OOR_MIN, - .max_sensor_pedal_1 = BRAKE1_PEDAL_OOR_MAX, - .max_sensor_pedal_2 = BRAKE1_PEDAL_OOR_MAX, - .activation_percentage = BRAKE_ACTIVATION_PERCENTAGE, - .deadzone_margin = DEFAULT_PEDAL_DEADZONE, - .implausibility_margin = DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, - .mechanical_activation_percentage = BRAKE_MECH_THRESH, -}; - -const PedalsParams brake2_only_params = { - .min_pedal_1 = BRAKE2_PEDAL_MIN, - .min_pedal_2 = BRAKE2_PEDAL_MIN, - .max_pedal_1 = BRAKE2_PEDAL_MAX, - .max_pedal_2 = BRAKE2_PEDAL_MAX, - .min_sensor_pedal_1 = BRAKE2_PEDAL_OOR_MIN, - .min_sensor_pedal_2 = BRAKE2_PEDAL_OOR_MIN, - .max_sensor_pedal_1 = BRAKE2_PEDAL_OOR_MAX, - .max_sensor_pedal_2 = BRAKE2_PEDAL_OOR_MAX, - .activation_percentage = BRAKE_ACTIVATION_PERCENTAGE, - .deadzone_margin = DEFAULT_PEDAL_DEADZONE, - .implausibility_margin = DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, - .mechanical_activation_percentage = BRAKE_MECH_THRESH, -}; - -const PedalsParams brake_params = { - .min_pedal_1 = BRAKE1_PEDAL_MIN, - .min_pedal_2 = BRAKE2_PEDAL_MIN, - .max_pedal_1 = BRAKE1_PEDAL_MAX, - .max_pedal_2 = BRAKE2_PEDAL_MAX, - .min_sensor_pedal_1 = BRAKE1_PEDAL_OOR_MIN, - .min_sensor_pedal_2 = BRAKE2_PEDAL_OOR_MIN, - .max_sensor_pedal_1 = BRAKE1_PEDAL_OOR_MAX, - .max_sensor_pedal_2 = BRAKE2_PEDAL_OOR_MAX, - .activation_percentage = BRAKE_ACTIVATION_PERCENTAGE, - .deadzone_margin = DEFAULT_PEDAL_DEADZONE, - .implausibility_margin = DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, - .mechanical_activation_percentage = BRAKE_MECH_THRESH, -}; - -/* - DATA SOURCES -*/ - -EthernetUDP protobuf_send_socket; -EthernetUDP protobuf_recv_socket; - -/* Two CAN lines on Main ECU rev15 */ -FlexCAN_T4 INV_CAN; // Inverter CAN (now both are on same line) -FlexCAN_T4 TELEM_CAN; // telemetry CAN (basically everything except inverters) - -/* Set up CAN circular buffer */ -// using CircularBufferType = Circular_Buffer; -using CircularBufferType = CANBufferType; - -/* Sensors */ -MCP_ADC<8> a1 = MCP_ADC<8>(ADC1_CS); -MCP_ADC<4> a2 = MCP_ADC<4>(ADC2_CS, 1000000); // 1M baud needed for 04s -MCP_ADC<4> a3 = MCP_ADC<4>(ADC3_CS, 1000000); -Teensy_ADC<2> mcu_adc = Teensy_ADC<2>(DEFAULT_ANALOG_PINS); -OrbisBR10 steering1(&Serial5); - -// /* -// INTERFACES -// */ -ETHInterfaces ethernet_interfaces = {}; -VNInterface vn_interface(&CAN3_txBuffer); -DashboardInterface dashboard(&CAN3_txBuffer); -AMSInterface ams_interface(&CAN3_txBuffer, SOFTWARE_OK); -WatchdogInterface wd_interface(WATCHDOG_INPUT); -MCUInterface main_ecu(&CAN3_txBuffer); -TelemetryInterface telem_interface(&CAN3_txBuffer, telem_read_channels); -ThermistorInterface front_thermistors_interface(&CAN3_txBuffer); -SABInterface sab_interface( - LOADCELL_RL_SCALE, // RL Scale - LOADCELL_RL_OFFSET, // RL Offset (Migos) - LOADCELL_RR_SCALE, // RR Scale - LOADCELL_RR_OFFSET // RR Offset -); -LoadCellInterface load_cell_interface; - -// /* Inverter Interface Type */ -using InvInt_t = InverterInterface; -struct inverters -{ - InvInt_t fl = InvInt_t(&CAN2_txBuffer, ID_MC1_SETPOINTS_COMMAND); - InvInt_t fr = InvInt_t(&CAN2_txBuffer, ID_MC2_SETPOINTS_COMMAND); - InvInt_t rl = InvInt_t(&CAN2_txBuffer, ID_MC3_SETPOINTS_COMMAND); - InvInt_t rr = InvInt_t(&CAN2_txBuffer, ID_MC4_SETPOINTS_COMMAND); -} inv; - -// /* -// SYSTEMS -// */ - -SysClock sys_clock; -SteeringSystem steering_system(&steering1, &telem_interface, STEERING_IIR_ALPHA); -BuzzerController buzzer(BUZZER_ON_INTERVAL); - -SafetySystem safety_system(&ams_interface, &wd_interface); -// SafetySystem safety_system(&ams_interface, &wd_interface, &dashboard); -PedalsSystem pedals_system(accel_params, brake_params); -using DriveSys_t = DrivetrainSystem; -DriveSys_t drivetrain = DriveSys_t({&inv.fl, &inv.fr, &inv.rl, &inv.rr}, &main_ecu, INVERTER_ENABLING_TIMEOUT_INTERVAL); -// TODO ensure that case uses max regen torque, right now its not -CASEConfiguration case_config = { - // Following used for generated code - .AbsoluteTorqueLimit = 21.42, // N-m, Torque limit used for yaw pid torque split overflow - .yaw_pid_p = 1.15, - .yaw_pid_i = 0.0, - .yaw_pid_d = 0.0, - .tcs_pid_p_lowerBound_front = 55.0, // if tcs_pid_p_lowerBound_front > tcs_pid_p_upperBound_front, inverse relationship, no error - .tcs_pid_p_upperBound_front = 42.0, - .tcs_pid_p_lowerBound_rear = 32.0, - .tcs_pid_p_upperBound_rear = 42.0, - .tcs_pid_i = 0.0, - .tcs_pid_d = 0.0, - .useLaunch = false, - .usePIDTV = true, - .useTCSLimitedYawPID = true, - .useNormalForce = true, - .useTractionControl = true, - .usePowerLimit = true, - .usePIDPowerLimit = false, - .useDecoupledYawBrakes = true, - .useDiscontinuousYawPIDBrakes = true, - .launchDeadZone = 20.0, // N-m - .launchVelThreshold = 0.15, // m/s - .tcsVelThreshold = 1.5, // m/s - .yawPIDMaxDifferential = 10.0, // N-m - .yawPIDErrorThreshold = 0.1, // rad/s - .yawPIDVelThreshold = 0.35, // m/s - .yawPIDCoastThreshold = 2.5, // m/s - .yaw_pid_brakes_p = 0.1, - .yaw_pid_brakes_i = 0.0, - .yaw_pid_brakes_d = 0.0, - .decoupledYawPIDBrakesMaxDIfference = 2, // N-m - .discontinuousBrakesPercentThreshold = 0.7, - .TorqueMode = 21.42, // N-m - .RegenLimit = -15.0, // N-m - .useNoRegen5kph = true, - .useTorqueBias = true, - .DriveTorquePercentFront = 0.5, // DON'T TOUCH UNTIL LOAD CELL ADHERES TO DRIVE BIAS - .BrakeTorquePercentFront = 0.6, - .MechPowerMaxkW = 51.5, // kW - .launchLeftRightMaxDiff = 2.0, // N-m - .tcs_pid_lower_rpm_front = 0.0, // RPM - .tcs_pid_upper_rpm_front = 5000.0, // RPM - .tcs_pid_lower_rpm_rear = 0.0, // RPM - .tcs_pid_upper_rpm_rear = 5000.0, // RPM - .maxNormalLoadBrakeScalingFront = 1.15, - .tcs_saturation_front = 20, - .tcs_saturation_rear = 20, - .TCSGenLeftRightDiffLowerBound = 2, // N-m - .TCSGenLeftRightDiffUpperBound = 20, // N-m - .TCSWheelSteerLowerBound = 2, // Deg - .TCSWheelSteerUpperBound = 25, // Deg - .useRPM_TCS_GainSchedule = true, // If both are false, then P values defaults to lower bound per axle - .useNL_TCS_GainSchedule = false, - .TCS_NL_startBoundPerc_FrontAxle = 0.5, - .TCS_NL_endBoundPerc_FrontAxle = 0.4, - .TCS_NL_startBoundPerc_RearAxle = 0.5, - .TCS_NL_endBoundPerc_RearAxle = 0.6, - .useNL_TCS_SlipSchedule = true, - .launchSL_startBound_Front = 0.25, - .launchSL_endBound_Front = 0.25, - .launchSL_startBound_Rear = 0.3, - .launchSL_endBound_Rear = 0.4, - .TCS_SL_startBound_Front = 0.25, - .TCS_SL_endBound_Front = 0.15, - .TCS_SL_startBound_Rear = 0.3, - .TCS_SL_endBound_Rear = 0.4, - .TCS_SL_NLPerc_startBound_Front = 0.5, - .TCS_SL_NLPerc_endBound_Front = 0.4, - .TCS_SL_NLPerc_startBound_Rear = 0.5, - .TCS_SL_NLPerc_endBound_Rear = 0.6, - - // Following used for calculate_torque_request in CASESystem.tpp - .max_rpm = 20000, - .max_regen_torque = 21.42, - .max_torque = 21.42, -}; -RateLimitedLogger logger; -//// Controllers -CASESystem case_system(&CAN3_txBuffer, 100, 70, 550, case_config); -// mode 0 -TorqueControllerSimple tc_simple(1.0f, 1.0f); -// mode 1 -TorqueControllerLoadCellVectoring tc_vec; -// mode 2 -TorqueControllerCASEWrapper case_wrapper(&case_system); - -// mode 3 -TorqueControllerSimpleLaunch simple_launch; -// mode 4 -DrivebrainController db_controller(210); - -TCMuxType torque_controller_mux({static_cast(&tc_simple), - static_cast(&tc_vec), - static_cast(&case_wrapper), - static_cast(&simple_launch), - static_cast(&db_controller)}, - {false, false, true, false, true}); - -/* Declare state machine */ -MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); - -// /* -// GROUPING STRUCTS (To limit parameter count in utilizing functions) -// */ - -DrivebrainETHInterface db_eth_interface; -CANInterfaces CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &vn_interface, &dashboard, &ams_interface, &sab_interface}; - -/* - FUNCTION DEFINITIONS -*/ - -/* Initialize CAN communication */ -void init_all_CAN_devices(); -/* Tick interfaces */ -void tick_all_interfaces(const SysTick_s ¤t_system_tick); -/* Tick all systems */ -void tick_all_systems(const SysTick_s ¤t_system_tick); -/* Reset inverters */ -void drivetrain_reset(); - -void handle_ethernet_interface_comms(const SysTick_s &systick, const hytech_msgs_MCUOutputData &out_msg); - -/* - SETUP -*/ - -void setup() -{ - // initialize CAN communication - init_all_CAN_devices(); - - Ethernet.begin(EthParams::default_MCU_MAC_address, EthParams::default_MCU_ip); - protobuf_send_socket.begin(EthParams::default_protobuf_send_port); - protobuf_recv_socket.begin(EthParams::default_protobuf_recv_port); - - SPI.begin(); - a1.init(); - a2.init(); - a3.init(); - mcu_adc.init(); - - a1.setChannelScale(MCU15_ACCEL1_CHANNEL, (1.0 / (float)(ACCEL1_PEDAL_MAX - ACCEL1_PEDAL_MIN))); - a1.setChannelScale(MCU15_ACCEL2_CHANNEL, (1.0 / (float)(ACCEL2_PEDAL_MAX - ACCEL2_PEDAL_MIN))); - a1.setChannelScale(MCU15_BRAKE1_CHANNEL, (1.0 / (float)(BRAKE1_PEDAL_MAX - BRAKE1_PEDAL_MIN))); - a1.setChannelScale(MCU15_BRAKE2_CHANNEL, (1.0 / (float)(BRAKE2_PEDAL_MAX - BRAKE2_PEDAL_MIN))); - a1.setChannelOffset(MCU15_ACCEL1_CHANNEL, -ACCEL1_PEDAL_MIN); - a1.setChannelOffset(MCU15_ACCEL2_CHANNEL, -ACCEL2_PEDAL_MIN); - a1.setChannelOffset(MCU15_BRAKE1_CHANNEL, -BRAKE1_PEDAL_MIN); - a1.setChannelOffset(MCU15_BRAKE2_CHANNEL, -BRAKE2_PEDAL_MIN); - a1.setChannelOffset(MCU15_STEERING_CHANNEL, -1 * SECONDARY_STEERING_SENSE_CENTER); - a1.setChannelScale(MCU15_STEERING_CHANNEL, STEERING_RANGE_DEGREES / ((float)SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND - (float)SECONDARY_STEERING_SENSE_LEFTMOST_BOUND)); - a1.setChannelClamp(MCU15_STEERING_CHANNEL, -STEERING_RANGE_DEGREES / 0.5 * 1.15, STEERING_RANGE_DEGREES / 0.5 * 1.15); // 15% tolerance on each end of the steering sensor - a1.setChannelScale(MCU15_GLV_SENSE_CHANNEL, GLV_SENSE_SCALE); - a2.setChannelScale(MCU15_FL_LOADCELL_CHANNEL, LOADCELL_FL_SCALE /*Todo*/); - a3.setChannelScale(MCU15_FR_LOADCELL_CHANNEL, LOADCELL_FR_SCALE /*Todo*/); - - a2.setChannelOffset(MCU15_FL_LOADCELL_CHANNEL, LOADCELL_FL_OFFSET /*Todo*/); - a3.setChannelOffset(MCU15_FR_LOADCELL_CHANNEL, LOADCELL_FR_OFFSET /*Todo*/); - - mcu_adc.setAlphas(MCU15_THERM_FL, 0.95); - mcu_adc.setAlphas(MCU15_THERM_FR, 0.95); - // get latest tick from sys clock - SysTick_s curr_tick = sys_clock.tick(micros()); - - /* - Init Interfaces - */ - - main_ecu.init(); // initial shutdown circuit readings, - wd_interface.init(curr_tick.millis); // initialize wd kick time - ams_interface.init(curr_tick); // initialize last heartbeat time - steering1.init(); - steering1.setOffset(PRIMARY_STEERING_SENSE_OFFSET); - - Serial.begin(115200); - - /* - Init Systems - */ - safety_system.init(); - - // Drivetrain set all inverters disabled - drivetrain.disable(); // write inv_en and inv_24V_en low: writing high in previous code though, should double check - // would an error list be good for debugging? i.e. which inverter has error - - // ControllerMux set max torque to 20 NM, torque mode to whatever makes most sense - // Preventing drivers from forgetting to toggle torque mode and end up self-derating at comp - // Not strictly necessary at the moment, just don't forget -} - -void loop() -{ - // get latest tick from sys clock - SysTick_s curr_tick = sys_clock.tick(micros()); - - // process received CAN messages - process_ring_buffer(CAN2_rxBuffer, CAN_receive_interfaces, curr_tick.millis); - process_ring_buffer(CAN3_rxBuffer, CAN_receive_interfaces, curr_tick.millis); - - // tick interfaces - tick_all_interfaces(curr_tick); - - // tick systems - - // single source of truth for the state of the car. - // no systems or interfaces should write directly to this. - SharedCarState_s car_state_inst(curr_tick, - steering_system.getSteeringSystemData(), - drivetrain.get_dynamic_data(), - load_cell_interface.getLoadCellForces(), - load_cell_interface.get_latest_raw_data(), - pedals_system.getPedalsSystemData(), - vn_interface.get_vn_struct(), - db_eth_interface.get_latest_data(), - torque_controller_mux.get_tc_mux_status(), - db_controller.get_timing_failure_status()); - - car_state_inst.drivebrain_timing_failure = db_controller.get_timing_failure_status(); - hytech_msgs_MCUOutputData out_eth_msg = db_eth_interface.make_db_msg(car_state_inst); - - handle_ethernet_interface_comms(curr_tick, out_eth_msg); - - tick_all_systems(curr_tick); - - // inverter procedure before entering state machine - // reset inverters - if (dashboard.inverterResetButtonPressed() && drivetrain.drivetrain_error_occured()) - { - drivetrain.reset_drivetrain(); - } - // tick state machine - - fsm.tick_state_machine(curr_tick.millis, car_state_inst); - - // tick safety system - safety_system.software_shutdown(curr_tick); - - // send CAN - send_all_CAN_msgs(CAN2_txBuffer, &INV_CAN); - send_all_CAN_msgs(CAN3_txBuffer, &TELEM_CAN); - - // Basic debug prints - // if (curr_tick.triggers.trigger5) - if (DEBUG_PRINTS) - { - Serial.print("Steering system reported angle (deg): "); - Serial.println(steering_system.getSteeringSystemData().angle); - Serial.print("Steering system status: "); - Serial.println(static_cast(steering_system.getSteeringSystemData().status)); - Serial.print("Primary sensor angle: "); - Serial.println(steering1.convert().angle); - Serial.print("Secondary sensor angle: "); - Serial.print(a1.get().conversions[MCU15_STEERING_CHANNEL].conversion); - Serial.print(" raw: "); - Serial.println(a1.get().conversions[MCU15_STEERING_CHANNEL].raw); - Serial.print("Sensor divergence: "); - Serial.println(steering1.convert().angle - a1.get().conversions[MCU15_STEERING_CHANNEL].conversion); - Serial.println(); - Serial.println("Pedal outputs:"); - Serial.print("Accel 1 raw: "); - Serial.println(a1.get().conversions[MCU15_ACCEL1_CHANNEL].raw); - Serial.print("Accel 2 raw: "); - Serial.println(a1.get().conversions[MCU15_ACCEL2_CHANNEL].raw); - Serial.print("Accel percent: "); - Serial.println(pedals_system.getPedalsSystemDataCopy().accelPercent); - Serial.print("Brake 1 raw: "); - Serial.println(a1.get().conversions[MCU15_BRAKE1_CHANNEL].raw); - Serial.print("Brake 2 raw: "); - Serial.println(a1.get().conversions[MCU15_BRAKE2_CHANNEL].raw); - Serial.print("Brake percent: "); - Serial.println(pedals_system.getPedalsSystemDataCopy().brakePercent); - Serial.println(); - Serial.print("Derating factor: "); - Serial.println(ams_interface.get_acc_derate_factor()); - Serial.print("Filtered min cell voltage: "); - Serial.println(ams_interface.get_filtered_min_cell_voltage()); - Serial.print("Filtered max cell temp: "); - Serial.println(ams_interface.get_filtered_max_cell_temp()); - Serial.print("Current TC index: "); - Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().active_controller_mode)); - Serial.print("Current TC error: "); - Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().active_error)); - Serial.println(); - Serial.print("dial state: "); - Serial.println(static_cast(dashboard.getDialMode())); - Serial.println(); - Serial.println(); - } -} - -/* - Initialize CAN comm. -*/ - -void init_all_CAN_devices() -{ - // Inverter CAN line - INV_CAN.begin(); - INV_CAN.setBaudRate(INV_CAN_BAUDRATE); - INV_CAN.setMaxMB(16); - INV_CAN.enableFIFO(); - INV_CAN.enableFIFOInterrupt(); - INV_CAN.onReceive(on_can2_receive); - INV_CAN.mailboxStatus(); - - // Telemetry CAN line - TELEM_CAN.begin(); - TELEM_CAN.setBaudRate(TELEM_CAN_BAUDRATE); - TELEM_CAN.setMaxMB(16); - TELEM_CAN.enableFIFO(); - TELEM_CAN.enableFIFOInterrupt(); - TELEM_CAN.onReceive(on_can3_receive); - TELEM_CAN.mailboxStatus(); -} - -/* - TICK INTERFACES -*/ - -void tick_all_interfaces(const SysTick_s ¤t_system_tick) -{ - - TriggerBits_s t = current_system_tick.triggers; - if (t.trigger10) // 10Hz - { - dashboard.tick10( - &main_ecu, - int(fsm.get_state()), - buzzer.buzzer_is_on(), - drivetrain.drivetrain_error_occured(), - torque_controller_mux.get_tc_mux_status().active_torque_limit_enum, - ams_interface.get_filtered_min_cell_voltage(), - a1.get().conversions[MCU15_GLV_SENSE_CHANNEL], - static_cast(torque_controller_mux.get_tc_mux_status().active_controller_mode), - dashboard.getDialMode()); - - main_ecu.tick( - static_cast(fsm.get_state()), - drivetrain.drivetrain_error_occured(), - safety_system.get_software_is_ok(), - torque_controller_mux.get_tc_mux_status(), - buzzer.buzzer_is_on(), - pedals_system.getPedalsSystemData(), - ams_interface.pack_charge_is_critical(), - dashboard.launchControlButtonPressed()); - - PedalsSystemData_s data2 = pedals_system.getPedalsSystemDataCopy(); - front_thermistors_interface.tick(mcu_adc.get().conversions[MCU15_THERM_FL_CHANNEL], mcu_adc.get().conversions[MCU15_THERM_FR_CHANNEL]); - // TODO pass in the shared state instead - telem_interface.tick( - a1.get(), - a2.get(), - a3.get(), - steering1.convert(), - &inv.fl, - &inv.fr, - &inv.rl, - &inv.rr, - data2.accelImplausible, - data2.brakeImplausible, - data2.accelPercent, - data2.brakePercent, - a1.get().conversions[MCU15_ACCEL1_CHANNEL], - a1.get().conversions[MCU15_ACCEL2_CHANNEL], - a1.get().conversions[MCU15_BRAKE1_CHANNEL], - a1.get().conversions[MCU15_BRAKE2_CHANNEL], - pedals_system.getMechBrakeActiveThreshold(), - torque_controller_mux.get_tc_mux_status().active_error); - - ams_interface.tick(current_system_tick); - - } - - if (t.trigger50) // 50Hz - { - steering1.sample(); - } - - if (t.trigger100) // 100Hz - { - - a1.tick(); - a2.tick(); - a3.tick(); - mcu_adc.tick(); - load_cell_interface.tick( - (LoadCellInterfaceTick_s){ - .FLConversion = a2.get().conversions[MCU15_FL_LOADCELL_CHANNEL], - .FRConversion = a3.get().conversions[MCU15_FR_LOADCELL_CHANNEL], - .RLConversion = sab_interface.rlLoadCell.convert(), - .RRConversion = sab_interface.rrLoadCell.convert()}); - } - load_cell_interface.update_raw_data((LoadCellInterfaceTick_s){ - .FLConversion = a2.get().conversions[MCU15_FL_LOADCELL_CHANNEL], - .FRConversion = a3.get().conversions[MCU15_FR_LOADCELL_CHANNEL], - .RLConversion = sab_interface.rlLoadCell.convert(), - .RRConversion = sab_interface.rrLoadCell.convert()}); - // // Untriggered - main_ecu.read_mcu_status(); // should be executed at the same rate as state machine - // DO NOT call in main_ecu.tick() -} - -// /* -// TICK SYSTEMS -// */ - -void tick_all_systems(const SysTick_s ¤t_system_tick) -{ - // tick pedals system - - pedals_system.tick( - current_system_tick, - a1.get().conversions[MCU15_ACCEL1_CHANNEL], - a1.get().conversions[MCU15_ACCEL2_CHANNEL], - a1.get().conversions[MCU15_BRAKE1_CHANNEL], - a1.get().conversions[MCU15_BRAKE2_CHANNEL]); - - // tick steering system - steering_system.tick( - (SteeringSystemTick_s){ - .tick = current_system_tick, - .secondaryConversion = a1.get().conversions[MCU15_STEERING_CHANNEL]}); - - // tick drivetrain system - drivetrain.tick(current_system_tick); - // // tick torque controller mux - - auto __attribute__((unused)) case_status = case_system.evaluate( - current_system_tick, - vn_interface.get_vn_struct(), - steering_system.getSteeringSystemData(), - drivetrain.get_dynamic_data(), - load_cell_interface.getLoadCellForces().loadCellConversions, // should CASE use filtered load cells? - pedals_system.getPedalsSystemData(), - 0, - fsm.get_state(), - dashboard.startButtonPressed(), - vn_interface.get_vn_struct().vn_status); -} - -void handle_ethernet_interface_comms(const SysTick_s &systick, const hytech_msgs_MCUOutputData &out_msg) -{ - // function that will handle receiving and distributing of all messages to all ethernet interfaces - // via the union message. this is a little bit cursed ngl. - - std::function recv_boi = &recv_pb_stream_msg; - handle_ethernet_socket_receive<1024, hytech_msgs_MCUCommandData>(systick, &protobuf_recv_socket, recv_boi, db_eth_interface, hytech_msgs_MCUCommandData_fields); - - if (systick.triggers.trigger1000) - { - handle_ethernet_socket_send_pb(EthParams::default_TCU_ip, EthParams::default_protobuf_send_port, &protobuf_send_socket, out_msg, hytech_msgs_MCUOutputData_fields); - } -} \ No newline at end of file diff --git a/src/main_ethernet_test.cpp b/src/main_ethernet_test.cpp deleted file mode 100644 index 064214c22..000000000 --- a/src/main_ethernet_test.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include - - -#include "ProtobufMsgInterface.h" -#include "MCU_rev15_defs.h" - -EthernetUDP protobuf_send_socket; -EthernetUDP protobuf_recv_socket; -ParameterInterface params = ParameterInterface(); -ETHInterfaces ethernet_interfaces = {¶ms}; - -void init_ethernet_device() -{ - Ethernet.begin(EthParams::default_MCU_MAC_address, EthParams::default_MCU_ip); - protobuf_send_socket.begin(EthParams::default_protobuf_send_port); - protobuf_recv_socket.begin(EthParams::default_protobuf_recv_port); -} - -void test_ethernet() -{ - - handle_ethernet_socket_receive(&protobuf_recv_socket, &recv_pb_stream_union_msg, ethernet_interfaces); - if(params.params_need_sending()) - { - auto config = params.get_config(); - if(!handle_ethernet_socket_send_pb(&protobuf_send_socket, config, config_fields)){ - } - params.reset_params_need_sending(); - } - -} - -void setup() -{ - init_ethernet_device(); -} - -void loop() -{ - test_ethernet(); - // Serial.println("loopin"); -} \ No newline at end of file diff --git a/src/main_spi_test.cpp b/src/main_spi_test.cpp deleted file mode 100644 index c814f46bb..000000000 --- a/src/main_spi_test.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include -#include "MCP_ADC.h" -const int SS_PIN = 34; // assuming MOSI 11, MISO 12, SCK 13 - - -MCP_ADC<8> a1 = MCP_ADC<8>(34); -MCP_ADC<4> a2 = MCP_ADC<4>(33); -MCP_ADC<4> a3 = MCP_ADC<4>(29); - - -void setup() -{ - SPI.begin(); - a1.init(); - a2.init(); - a3.init(); -} - -void loop() -{ - Serial.println("looped"); - a1.tick(); - a2.tick(); - a3.tick(); -} diff --git a/src/test_can_interface.cpp b/src/test_can_interface.cpp deleted file mode 100644 index aed1b1ef4..000000000 --- a/src/test_can_interface.cpp +++ /dev/null @@ -1,137 +0,0 @@ - -#include -#include "HytechCANInterface.h" -#include "InverterInterface.h" - -FlexCAN_T4 TEST_CAN1; -FlexCAN_T4 TEST_CAN2; // Inverter CAN (now both are on same line) - -CAN_message_t msg; - -using CircularBufferType = CANBufferType; -using InverterInterfaceType = InverterInterface; -InverterInterfaceType fl_inv = InverterInterfaceType(&CAN2_txBuffer, MC1_SETPOINTS_COMMAND_CANID); -InverterInterfaceType fr_inv = InverterInterfaceType(&CAN2_txBuffer, MC2_SETPOINTS_COMMAND_CANID); -InverterInterfaceType rl_inv = InverterInterfaceType(&CAN2_txBuffer, MC3_SETPOINTS_COMMAND_CANID); -InverterInterfaceType rr_inv = InverterInterfaceType(&CAN2_txBuffer, MC4_SETPOINTS_COMMAND_CANID); - -CANInterfaces CAN_interfaces = {&fl_inv, &fr_inv, &rl_inv, &rr_inv, 0, 0, 0}; - -void init_can_interface() -{ - TEST_CAN2.begin(); - TEST_CAN2.setBaudRate(500000); - TEST_CAN2.setMaxMB(16); - TEST_CAN2.enableFIFO(); - TEST_CAN2.enableFIFOInterrupt(); - TEST_CAN2.onReceive(on_can2_receive); - TEST_CAN2.mailboxStatus(); - - TEST_CAN1.begin(); - TEST_CAN1.setBaudRate(500000); - TEST_CAN1.setMaxMB(16); - TEST_CAN1.enableFIFO(); - TEST_CAN1.enableFIFOInterrupt(); - TEST_CAN1.enableMBInterrupts(); - TEST_CAN1.onReceive(on_can1_receive); - TEST_CAN1.mailboxStatus(); -} - -void setup(void) -{ - while (!Serial) - { - // wait for Arduino Serial Monitor to be ready - } - init_can_interface(); -} -bool ran_test = false; -void loop() -{ - // test for seeing if when CAN2 is connected to CAN1 the can1 rx buffer will get stuff - // testing whether or not a CAN message can be sent - - if (!ran_test) - { - msg.id = random(0x1, 0x7FE); - for (uint8_t i = 0; i < 8; i++) - msg.buf[i] = i + 1; - TEST_CAN2.write(msg); - ran_test = true; - delay(10); - Serial.println("======= TEST 1 ======="); - Serial.println("can1 rx buffer size: (should be 1)"); - Serial.println(CAN1_rxBuffer.available()); - Serial.println("======= TEST 2 ======="); - - Serial.println("can1 received message equal to sent msg"); - delay(10); - if (CAN1_rxBuffer.available()) - { - CAN_message_t recvd_msg; - uint8_t buf[sizeof(CAN_message_t)]; - CAN1_rxBuffer.pop_front(buf, sizeof(CAN_message_t)); - memmove(&recvd_msg, buf, sizeof(recvd_msg)); - - Serial.println("\tid = prev: (should be 1)"); - Serial.print("\t"); - Serial.println(recvd_msg.id == msg.id); - } - - Serial.println("======= TEST 3 ======="); - // command the rl inverter to reset (sets the remove error flag on mc setpoints command) - // queues onto tx buffer - rl_inv.command_reset(); - // msg.id = random(0x1, 0x7FE); - // for (uint8_t i = 0; i < 8; i++) msg.buf[i] = i + 1; - // uint8_t buf2[sizeof(CAN_message_t)]; - // memmove(buf2, &msg, sizeof(msg)); - // CAN2_txBuffer.push_back(buf2, sizeof(CAN_message_t)); - // send all messages on tx buffer - - Serial.println("can2 tx buffer size: (should be 1)"); - - Serial.println(CAN2_txBuffer.available()); - send_all_CAN_msgs(CAN2_txBuffer, &TEST_CAN2); - - Serial.println("can2 tx buffer size: (should be 0)"); - - Serial.println(CAN2_txBuffer.available()); - Serial.println("sent on can2"); - delay(10); - Serial.println("can1 rx buffer size: (should be 1) "); - Serial.print(CAN1_rxBuffer.available()); - Serial.println(); - process_ring_buffer(CAN1_rxBuffer, CAN_interfaces, 1000); - - Serial.println("sent on can2"); - Serial.println("can1 rx buffer size: (should be 0) "); - Serial.println(CAN1_rxBuffer.available()); - - Serial.println("======= TEST 4 ======="); - - rl_inv.command_reset(); - send_all_CAN_msgs(CAN2_txBuffer, &TEST_CAN2); - - delay(10); - CAN_message_t recvd_msg; - uint8_t buf[sizeof(CAN_message_t)]; - CAN1_rxBuffer.pop_front(buf, sizeof(CAN_message_t)); - memmove(&recvd_msg, buf, sizeof(recvd_msg)); - if (recvd_msg.id == ID_MC3_SETPOINTS_COMMAND) - { - MC_setpoints_command msg; - msg.load(recvd_msg.buf); - if (msg.get_remove_error()) - { - - Serial.println("GOOD received correct inverter reset"); - } - } else{ - - Serial.println("ERROR didnt receive correct id"); - Serial.println(recvd_msg.id); - Serial.println(ID_MC3_SETPOINTS_COMMAND); - } - } -} \ No newline at end of file