MCU
Loading...
Searching...
No Matches
Classes | Macros | Typedefs | Functions | Variables
main.cpp File Reference
#include <Arduino.h>
#include "FlexCAN_T4.h"
#include "HyTech_CAN.h"
#include "MCU_rev15_defs.h"
#include "pb.h"
#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"
#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"
#include "MCUStateMachine.h"
#include "HT08_CASE.h"
#include "InterfaceParams.h"
#include "PrintLogger.h"
#include "hytech_msgs.pb.h"
Include dependency graph for main.cpp:

Go to the source code of this file.

Classes

struct  inverters
 

Macros

#define DEBUG_PRINTS   false
 

Typedefs

using CircularBufferType = CANBufferType
 
using InvInt_t = InverterInterface< CircularBufferType >
 
using DriveSys_t = DrivetrainSystem< InvInt_t >
 

Functions

void init_all_CAN_devices ()
 
void tick_all_interfaces (const SysTick_s &current_system_tick)
 
void tick_all_systems (const SysTick_s &current_system_tick)
 
void drivetrain_reset ()
 
void handle_ethernet_interface_comms (const SysTick_s &systick, const hytech_msgs_MCUOutputData &out_msg)
 
void setup ()
 
void loop ()
 

Variables

const TelemetryInterfaceReadChannels telem_read_channels
 
const PedalsParams accel1_only_params
 
const PedalsParams accel2_only_params
 
const PedalsParams accel_params
 
const PedalsParams brake1_only_params
 
const PedalsParams brake2_only_params
 
const PedalsParams brake_params
 
EthernetUDP protobuf_send_socket
 
EthernetUDP protobuf_recv_socket
 
FlexCAN_T4< CAN2, RX_SIZE_256, TX_SIZE_16 > INV_CAN
 
FlexCAN_T4< CAN3, RX_SIZE_256, TX_SIZE_16 > TELEM_CAN
 
MCP_ADC< 8 > a1 = MCP_ADC<8>(ADC1_CS)
 
MCP_ADC< 4 > a2 = MCP_ADC<4>(ADC2_CS, 1000000)
 
MCP_ADC< 4 > a3 = MCP_ADC<4>(ADC3_CS, 1000000)
 
Teensy_ADC< 2 > mcu_adc = Teensy_ADC<2>(DEFAULT_ANALOG_PINS)
 
OrbisBR10 steering1Serial5
 
ETHInterfaces ethernet_interfaces = {}
 
VNInterface< CircularBufferType > vn_interface & CAN3_txBuffer
 
WatchdogInterface wd_interface (WATCHDOG_INPUT)
 
SABInterface sab_interface (LOADCELL_RL_SCALE, LOADCELL_RL_OFFSET, LOADCELL_RR_SCALE, LOADCELL_RR_OFFSET)
 
LoadCellInterface load_cell_interface
 
struct inverters inv
 
SysClock sys_clock
 
SteeringSystem steering_system & steering1
 
BuzzerController buzzer (BUZZER_ON_INTERVAL)
 
SafetySystem safety_system & ams_interface
 
PedalsSystem pedals_system (accel_params, brake_params)
 
DriveSys_t drivetrain = DriveSys_t({&inv.fl, &inv.fr, &inv.rl, &inv.rr}, &main_ecu, INVERTER_ENABLING_TIMEOUT_INTERVAL)
 
CASEConfiguration case_config
 
RateLimitedLogger logger
 
TorqueControllerSimple tc_simple (1.0f, 1.0f)
 
TorqueControllerLoadCellVectoring tc_vec
 
TorqueControllerCASEWrapper< CircularBufferType > case_wrapper & case_system
 
TorqueControllerSimpleLaunch simple_launch
 
DrivebrainController db_controller (210)
 
TCMuxType torque_controller_mux ({static_cast< Controller * >(&tc_simple), static_cast< Controller * >(&tc_vec), static_cast< Controller * >(&case_wrapper), static_cast< Controller * >(&simple_launch), static_cast< Controller * >(&db_controller)}, {false, false, true, false, true})
 
DrivebrainETHInterface db_eth_interface
 
CANInterfaces< CircularBufferTypeCAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &vn_interface, &dashboard, &ams_interface, &sab_interface}
 

Macro Definition Documentation

◆ DEBUG_PRINTS

#define DEBUG_PRINTS   false

Definition at line 4 of file main.cpp.

Typedef Documentation

◆ CircularBufferType

Definition at line 173 of file main.cpp.

◆ DriveSys_t

Definition at line 222 of file main.cpp.

◆ InvInt_t

Definition at line 202 of file main.cpp.

Function Documentation

◆ drivetrain_reset()

void drivetrain_reset ( )

◆ handle_ethernet_interface_comms()

void handle_ethernet_interface_comms ( const SysTick_s &  systick,
const hytech_msgs_MCUOutputData &  out_msg 
)

Definition at line 667 of file main.cpp.

668{
669 // function that will handle receiving and distributing of all messages to all ethernet interfaces
670 // via the union message. this is a little bit cursed ngl.
671
672 std::function<void(unsigned long, const uint8_t *, size_t, DrivebrainETHInterface &, const pb_msgdesc_t *)> recv_boi = &recv_pb_stream_msg<hytech_msgs_MCUCommandData, DrivebrainETHInterface>;
673 handle_ethernet_socket_receive<1024, hytech_msgs_MCUCommandData>(systick, &protobuf_recv_socket, recv_boi, db_eth_interface, hytech_msgs_MCUCommandData_fields);
674
675 if (systick.triggers.trigger1000)
676 {
677 handle_ethernet_socket_send_pb<hytech_msgs_MCUOutputData, 1024>(EthParams::default_TCU_ip, EthParams::default_protobuf_send_port, &protobuf_send_socket, out_msg, hytech_msgs_MCUOutputData_fields);
678 }
679}
const uint16_t default_protobuf_send_port
const IPAddress default_TCU_ip(192, 168, 1, 69)
EthernetUDP protobuf_send_socket
Definition: main.cpp:164
DrivebrainETHInterface db_eth_interface
Definition: main.cpp:331
EthernetUDP protobuf_recv_socket
Definition: main.cpp:165

◆ init_all_CAN_devices()

void init_all_CAN_devices ( )

Definition at line 521 of file main.cpp.

522{
523 // Inverter CAN line
524 INV_CAN.begin();
525 INV_CAN.setBaudRate(INV_CAN_BAUDRATE);
526 INV_CAN.setMaxMB(16);
527 INV_CAN.enableFIFO();
528 INV_CAN.enableFIFOInterrupt();
529 INV_CAN.onReceive(on_can2_receive);
530 INV_CAN.mailboxStatus();
531
532 // Telemetry CAN line
533 TELEM_CAN.begin();
534 TELEM_CAN.setBaudRate(TELEM_CAN_BAUDRATE);
535 TELEM_CAN.setMaxMB(16);
536 TELEM_CAN.enableFIFO();
537 TELEM_CAN.enableFIFOInterrupt();
538 TELEM_CAN.onReceive(on_can3_receive);
539 TELEM_CAN.mailboxStatus();
540}
const unsigned long TELEM_CAN_BAUDRATE
const unsigned long INV_CAN_BAUDRATE
void on_can2_receive(const CAN_message_t &msg)
void on_can3_receive(const CAN_message_t &msg)
FlexCAN_T4< CAN2, RX_SIZE_256, TX_SIZE_16 > INV_CAN
Definition: main.cpp:168
FlexCAN_T4< CAN3, RX_SIZE_256, TX_SIZE_16 > TELEM_CAN
Definition: main.cpp:169

◆ loop()

void loop ( )

Definition at line 417 of file main.cpp.

418{
419 // get latest tick from sys clock
420 SysTick_s curr_tick = sys_clock.tick(micros());
421
422 // process received CAN messages
425
426 // tick interfaces
427 tick_all_interfaces(curr_tick);
428
429 // tick systems
430
431 // single source of truth for the state of the car.
432 // no systems or interfaces should write directly to this.
433 SharedCarState_s car_state_inst(curr_tick,
434 steering_system.getSteeringSystemData(),
439 vn_interface.get_vn_struct(),
443
444 car_state_inst.drivebrain_timing_failure = db_controller.get_timing_failure_status();
445 hytech_msgs_MCUOutputData out_eth_msg = db_eth_interface.make_db_msg(car_state_inst);
446
447 handle_ethernet_interface_comms(curr_tick, out_eth_msg);
448
449 tick_all_systems(curr_tick);
450
451 // inverter procedure before entering state machine
452 // reset inverters
453 if (dashboard.inverterResetButtonPressed() && drivetrain.drivetrain_error_occured())
454 {
456 }
457 // tick state machine
458
459 fsm.tick_state_machine(curr_tick.millis, car_state_inst);
460
461 // tick safety system
462 safety_system.software_shutdown(curr_tick);
463
464 // send CAN
467
468 // Basic debug prints
469 // if (curr_tick.triggers.trigger5)
470 if (DEBUG_PRINTS)
471 {
472 Serial.print("Steering system reported angle (deg): ");
473 Serial.println(steering_system.getSteeringSystemData().angle);
474 Serial.print("Steering system status: ");
475 Serial.println(static_cast<uint8_t>(steering_system.getSteeringSystemData().status));
476 Serial.print("Primary sensor angle: ");
477 Serial.println(steering1.convert().angle);
478 Serial.print("Secondary sensor angle: ");
479 Serial.print(a1.get().conversions[MCU15_STEERING_CHANNEL].conversion);
480 Serial.print(" raw: ");
481 Serial.println(a1.get().conversions[MCU15_STEERING_CHANNEL].raw);
482 Serial.print("Sensor divergence: ");
483 Serial.println(steering1.convert().angle - a1.get().conversions[MCU15_STEERING_CHANNEL].conversion);
484 Serial.println();
485 Serial.println("Pedal outputs:");
486 Serial.print("Accel 1 raw: ");
487 Serial.println(a1.get().conversions[MCU15_ACCEL1_CHANNEL].raw);
488 Serial.print("Accel 2 raw: ");
489 Serial.println(a1.get().conversions[MCU15_ACCEL2_CHANNEL].raw);
490 Serial.print("Accel percent: ");
492 Serial.print("Brake 1 raw: ");
493 Serial.println(a1.get().conversions[MCU15_BRAKE1_CHANNEL].raw);
494 Serial.print("Brake 2 raw: ");
495 Serial.println(a1.get().conversions[MCU15_BRAKE2_CHANNEL].raw);
496 Serial.print("Brake percent: ");
498 Serial.println();
499 Serial.print("Derating factor: ");
500 Serial.println(ams_interface.get_acc_derate_factor());
501 Serial.print("Filtered min cell voltage: ");
502 Serial.println(ams_interface.get_filtered_min_cell_voltage());
503 Serial.print("Filtered max cell temp: ");
504 Serial.println(ams_interface.get_filtered_max_cell_temp());
505 Serial.print("Current TC index: ");
506 Serial.println(static_cast<int>(torque_controller_mux.get_tc_mux_status().active_controller_mode));
507 Serial.print("Current TC error: ");
508 Serial.println(static_cast<int>(torque_controller_mux.get_tc_mux_status().active_error));
509 Serial.println();
510 Serial.print("dial state: ");
511 Serial.println(static_cast<int>(dashboard.getDialMode()));
512 Serial.println();
513 Serial.println();
514 }
515}
const int MCU15_BRAKE1_CHANNEL
const int MCU15_STEERING_CHANNEL
const int MCU15_ACCEL2_CHANNEL
const int MCU15_BRAKE2_CHANNEL
const int MCU15_ACCEL1_CHANNEL
bool get_timing_failure_status()
getter for the current status of whether or not the controller has had a timing failure during operat...
hytech_msgs_MCUOutputData make_db_msg(const SharedCarState_s &shared_state)
DrivebrainData_s get_latest_data()
bool drivetrain_error_occured()
DrivetrainDynamicReport_s get_dynamic_data()
void reset_drivetrain()
LoadCellInterfaceOutput_s getLoadCellForces()
LoadCellInterfaceRawOutput_s get_latest_raw_data()
PedalsSystemData_s getPedalsSystemDataCopy()
Definition: PedalsSystem.h:67
const PedalsSystemData_s & getPedalsSystemData()
Definition: PedalsSystem.h:62
const TorqueControllerMuxStatus & get_tc_mux_status()
Circular_Buffer< uint8_t,(uint32_t) 16, sizeof(CAN_message_t)> CAN3_rxBuffer
void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, unsigned long curr_millis)
void send_all_CAN_msgs(bufferType &buffer, FlexCAN_T4_Base *can_interface)
Circular_Buffer< uint8_t,(uint32_t) 16, sizeof(CAN_message_t)> CAN2_rxBuffer
CANBufferType CAN2_txBuffer
LoadCellInterface load_cell_interface
Definition: main.cpp:199
SteeringSystem steering_system & steering1
Definition: main.cpp:216
PedalsSystem pedals_system(accel_params, brake_params)
DrivebrainController db_controller(210)
TCMuxType torque_controller_mux({static_cast< Controller * >(&tc_simple), static_cast< Controller * >(&tc_vec), static_cast< Controller * >(&case_wrapper), static_cast< Controller * >(&simple_launch), static_cast< Controller * >(&db_controller)}, {false, false, true, false, true})
#define DEBUG_PRINTS
Definition: main.cpp:4
void tick_all_interfaces(const SysTick_s &current_system_tick)
Definition: main.cpp:546
DriveSys_t drivetrain
Definition: main.cpp:223
CANInterfaces< CircularBufferType > CAN_receive_interfaces
Definition: main.cpp:332
VNInterface< CircularBufferType > vn_interface & CAN3_txBuffer
Definition: main.cpp:186
MCP_ADC< 8 > a1
Definition: main.cpp:176
void tick_all_systems(const SysTick_s &current_system_tick)
Definition: main.cpp:633
void handle_ethernet_interface_comms(const SysTick_s &systick, const hytech_msgs_MCUOutputData &out_msg)
Definition: main.cpp:667
SafetySystem safety_system & ams_interface
Definition: main.cpp:219
SysClock sys_clock
Definition: main.cpp:215
car state struct that contains state of everything about the car including
TorqueControllerMuxError active_error
ControllerMode_e active_controller_mode

◆ setup()

void setup ( void  )

Definition at line 353 of file main.cpp.

354{
355 // initialize CAN communication
357
361
362 SPI.begin();
363 a1.init();
364 a2.init();
365 a3.init();
366 mcu_adc.init();
367
368 a1.setChannelScale(MCU15_ACCEL1_CHANNEL, (1.0 / (float)(ACCEL1_PEDAL_MAX - ACCEL1_PEDAL_MIN)));
369 a1.setChannelScale(MCU15_ACCEL2_CHANNEL, (1.0 / (float)(ACCEL2_PEDAL_MAX - ACCEL2_PEDAL_MIN)));
370 a1.setChannelScale(MCU15_BRAKE1_CHANNEL, (1.0 / (float)(BRAKE1_PEDAL_MAX - BRAKE1_PEDAL_MIN)));
371 a1.setChannelScale(MCU15_BRAKE2_CHANNEL, (1.0 / (float)(BRAKE2_PEDAL_MAX - BRAKE2_PEDAL_MIN)));
372 a1.setChannelOffset(MCU15_ACCEL1_CHANNEL, -ACCEL1_PEDAL_MIN);
373 a1.setChannelOffset(MCU15_ACCEL2_CHANNEL, -ACCEL2_PEDAL_MIN);
374 a1.setChannelOffset(MCU15_BRAKE1_CHANNEL, -BRAKE1_PEDAL_MIN);
375 a1.setChannelOffset(MCU15_BRAKE2_CHANNEL, -BRAKE2_PEDAL_MIN);
378 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
380 a2.setChannelScale(MCU15_FL_LOADCELL_CHANNEL, LOADCELL_FL_SCALE /*Todo*/);
381 a3.setChannelScale(MCU15_FR_LOADCELL_CHANNEL, LOADCELL_FR_SCALE /*Todo*/);
382
383 a2.setChannelOffset(MCU15_FL_LOADCELL_CHANNEL, LOADCELL_FL_OFFSET /*Todo*/);
384 a3.setChannelOffset(MCU15_FR_LOADCELL_CHANNEL, LOADCELL_FR_OFFSET /*Todo*/);
385
386 mcu_adc.setAlphas(MCU15_THERM_FL, 0.95);
387 mcu_adc.setAlphas(MCU15_THERM_FR, 0.95);
388 // get latest tick from sys clock
389 SysTick_s curr_tick = sys_clock.tick(micros());
390
391 /*
392 Init Interfaces
393 */
394
395 main_ecu.init(); // initial shutdown circuit readings,
396 wd_interface.init(curr_tick.millis); // initialize wd kick time
397 ams_interface.init(curr_tick); // initialize last heartbeat time
398 steering1.init();
400
401 Serial.begin(115200);
402
403 /*
404 Init Systems
405 */
406 safety_system.init();
407
408 // Drivetrain set all inverters disabled
409 drivetrain.disable(); // write inv_en and inv_24V_en low: writing high in previous code though, should double check
410 // would an error list be good for debugging? i.e. which inverter has error
411
412 // ControllerMux set max torque to 20 NM, torque mode to whatever makes most sense
413 // Preventing drivers from forgetting to toggle torque mode and end up self-derating at comp
414 // Not strictly necessary at the moment, just don't forget
415}
const float LOADCELL_FL_SCALE
const int MCU15_THERM_FR
const float LOADCELL_FL_OFFSET
const int ACCEL1_PEDAL_MIN
const int BRAKE1_PEDAL_MAX
const int BRAKE2_PEDAL_MIN
const int MCU15_GLV_SENSE_CHANNEL
const int BRAKE1_PEDAL_MIN
const int ACCEL1_PEDAL_MAX
const float GLV_SENSE_SCALE
const float STEERING_RANGE_DEGREES
const int MCU15_THERM_FL
const int ACCEL2_PEDAL_MIN
const int ACCEL2_PEDAL_MAX
const float LOADCELL_FR_SCALE
const int MCU15_FR_LOADCELL_CHANNEL
const int MCU15_FL_LOADCELL_CHANNEL
const int SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND
const float PRIMARY_STEERING_SENSE_OFFSET
const int BRAKE2_PEDAL_MAX
const float LOADCELL_FR_OFFSET
const int SECONDARY_STEERING_SENSE_LEFTMOST_BOUND
const int SECONDARY_STEERING_SENSE_CENTER
void init(unsigned long curr_millis)
uint8_t default_MCU_MAC_address[6]
const uint16_t default_protobuf_recv_port
const IPAddress default_MCU_ip(192, 168, 1, 30)
Teensy_ADC< 2 > mcu_adc
Definition: main.cpp:179
MCP_ADC< 4 > a2
Definition: main.cpp:177
WatchdogInterface wd_interface(WATCHDOG_INPUT)
void init_all_CAN_devices()
Definition: main.cpp:521
MCP_ADC< 4 > a3
Definition: main.cpp:178

◆ tick_all_interfaces()

void tick_all_interfaces ( const SysTick_s &  current_system_tick)

Definition at line 546 of file main.cpp.

547{
548
549 TriggerBits_s t = current_system_tick.triggers;
550 if (t.trigger10) // 10Hz
551 {
552 dashboard.tick10(
553 &main_ecu,
554 int(fsm.get_state()),
558 ams_interface.get_filtered_min_cell_voltage(),
559 a1.get().conversions[MCU15_GLV_SENSE_CHANNEL],
561 dashboard.getDialMode());
562
563 main_ecu.tick(
564 static_cast<int>(fsm.get_state()),
566 safety_system.get_software_is_ok(),
570 ams_interface.pack_charge_is_critical(),
571 dashboard.launchControlButtonPressed());
572
574 front_thermistors_interface.tick(mcu_adc.get().conversions[MCU15_THERM_FL_CHANNEL], mcu_adc.get().conversions[MCU15_THERM_FR_CHANNEL]);
575 // TODO pass in the shared state instead
576 telem_interface.tick(
577 a1.get(),
578 a2.get(),
579 a3.get(),
580 steering1.convert(),
581 &inv.fl,
582 &inv.fr,
583 &inv.rl,
584 &inv.rr,
585 data2.accelImplausible,
586 data2.brakeImplausible,
587 data2.accelPercent,
588 data2.brakePercent,
589 a1.get().conversions[MCU15_ACCEL1_CHANNEL],
590 a1.get().conversions[MCU15_ACCEL2_CHANNEL],
591 a1.get().conversions[MCU15_BRAKE1_CHANNEL],
592 a1.get().conversions[MCU15_BRAKE2_CHANNEL],
595
596 ams_interface.tick(current_system_tick);
597
598 }
599
600 if (t.trigger50) // 50Hz
601 {
602 steering1.sample();
603 }
604
605 if (t.trigger100) // 100Hz
606 {
607
608 a1.tick();
609 a2.tick();
610 a3.tick();
611 mcu_adc.tick();
614 .FLConversion = a2.get().conversions[MCU15_FL_LOADCELL_CHANNEL],
615 .FRConversion = a3.get().conversions[MCU15_FR_LOADCELL_CHANNEL],
616 .RLConversion = sab_interface.rlLoadCell.convert(),
617 .RRConversion = sab_interface.rrLoadCell.convert()});
618 }
620 .FLConversion = a2.get().conversions[MCU15_FL_LOADCELL_CHANNEL],
621 .FRConversion = a3.get().conversions[MCU15_FR_LOADCELL_CHANNEL],
622 .RLConversion = sab_interface.rlLoadCell.convert(),
623 .RRConversion = sab_interface.rrLoadCell.convert()});
624 // // Untriggered
625 main_ecu.read_mcu_status(); // should be executed at the same rate as state machine
626 // DO NOT call in main_ecu.tick()
627}
const int MCU15_THERM_FR_CHANNEL
const int MCU15_THERM_FL_CHANNEL
AnalogConversion_s convert()
Calculate sensor output and whether result is in sensor's defined bounds. DOES NOT SAMPLE.
bool buzzer_is_on()
Definition: Buzzer.h:16
void tick(const LoadCellInterfaceTick_s &intake)
void update_raw_data(const LoadCellInterfaceTick_s &intake)
float getMechBrakeActiveThreshold()
Definition: PedalsSystem.h:72
AnalogChannel rlLoadCell
Definition: SABInterface.h:12
AnalogChannel rrLoadCell
Definition: SABInterface.h:13
SABInterface sab_interface(LOADCELL_RL_SCALE, LOADCELL_RL_OFFSET, LOADCELL_RR_SCALE, LOADCELL_RR_OFFSET)
struct inverters inv
BuzzerController buzzer(BUZZER_ON_INTERVAL)
Definition: main.cpp:325
TorqueLimit_e active_torque_limit_enum
InvInt_t rr
Definition: main.cpp:208
InvInt_t rl
Definition: main.cpp:207
InvInt_t fr
Definition: main.cpp:206
InvInt_t fl
Definition: main.cpp:205

◆ tick_all_systems()

void tick_all_systems ( const SysTick_s &  current_system_tick)

Definition at line 633 of file main.cpp.

634{
635 // tick pedals system
636
638 current_system_tick,
639 a1.get().conversions[MCU15_ACCEL1_CHANNEL],
640 a1.get().conversions[MCU15_ACCEL2_CHANNEL],
641 a1.get().conversions[MCU15_BRAKE1_CHANNEL],
642 a1.get().conversions[MCU15_BRAKE2_CHANNEL]);
643
644 // tick steering system
645 steering_system.tick(
647 .tick = current_system_tick,
648 .secondaryConversion = a1.get().conversions[MCU15_STEERING_CHANNEL]});
649
650 // tick drivetrain system
651 drivetrain.tick(current_system_tick);
652 // // tick torque controller mux
653
654 auto __attribute__((unused)) case_status = case_system.evaluate(
655 current_system_tick,
656 vn_interface.get_vn_struct(),
657 steering_system.getSteeringSystemData(),
659 load_cell_interface.getLoadCellForces().loadCellConversions, // should CASE use filtered load cells?
661 0,
662 fsm.get_state(),
663 dashboard.startButtonPressed(),
664 vn_interface.get_vn_struct().vn_status);
665}
void tick(const SysTick_s &tick)
void tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake)
overloaded tick function that runs the evaluation of the pedals system. evaluates brake using only mi...
TorqueControllerCASEWrapper< CircularBufferType > case_wrapper & case_system
Definition: main.cpp:310
veh_vec< AnalogConversion_s > loadCellConversions

Variable Documentation

◆ a1

MCP_ADC<8> a1 = MCP_ADC<8>(ADC1_CS)

Definition at line 176 of file main.cpp.

◆ a2

MCP_ADC<4> a2 = MCP_ADC<4>(ADC2_CS, 1000000)

Definition at line 177 of file main.cpp.

◆ a3

MCP_ADC<4> a3 = MCP_ADC<4>(ADC3_CS, 1000000)

Definition at line 178 of file main.cpp.

◆ accel1_only_params

const PedalsParams accel1_only_params
Initial value:
= {
.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 float DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN
const int ACCEL1_PEDAL_OOR_MAX
const float DEFAULT_PEDAL_DEADZONE
const float APPS_ACTIVATION_PERCENTAGE
const int ACCEL1_PEDAL_OOR_MIN

Definition at line 73 of file main.cpp.

◆ accel2_only_params

const PedalsParams accel2_only_params
Initial value:
= {
.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 int ACCEL2_PEDAL_OOR_MIN
const int ACCEL2_PEDAL_OOR_MAX

Definition at line 87 of file main.cpp.

◆ accel_params

const PedalsParams accel_params
Initial value:
= {
.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}

Definition at line 101 of file main.cpp.

◆ ams_interface

SafetySystem safety_system& ams_interface

Definition at line 219 of file main.cpp.

◆ brake1_only_params

const PedalsParams brake1_only_params
Initial value:
= {
.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 float BRAKE_ACTIVATION_PERCENTAGE
const int BRAKE1_PEDAL_OOR_MIN
const float BRAKE_MECH_THRESH
const int BRAKE1_PEDAL_OOR_MAX

Definition at line 115 of file main.cpp.

◆ brake2_only_params

const PedalsParams brake2_only_params
Initial value:
= {
.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 int BRAKE2_PEDAL_OOR_MIN
const int BRAKE2_PEDAL_OOR_MAX

Definition at line 130 of file main.cpp.

◆ brake_params

const PedalsParams brake_params
Initial value:
= {
.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,
}

Definition at line 145 of file main.cpp.

◆ buzzer

Definition at line 325 of file main.cpp.

◆ CAN3_txBuffer

Definition at line 186 of file main.cpp.

◆ CAN_receive_interfaces

CANInterfaces<CircularBufferType> CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &vn_interface, &dashboard, &ams_interface, &sab_interface}

Definition at line 332 of file main.cpp.

◆ case_config

CASEConfiguration case_config

Definition at line 225 of file main.cpp.

◆ case_system

TorqueControllerCASEWrapper<CircularBufferType> case_wrapper& case_system

Definition at line 310 of file main.cpp.

◆ db_controller

DrivebrainController db_controller(210) ( 210  )

◆ db_eth_interface

DrivebrainETHInterface db_eth_interface

Definition at line 331 of file main.cpp.

◆ drivetrain

DriveSys_t drivetrain = DriveSys_t({&inv.fl, &inv.fr, &inv.rl, &inv.rr}, &main_ecu, INVERTER_ENABLING_TIMEOUT_INTERVAL)

Definition at line 223 of file main.cpp.

◆ ethernet_interfaces

ETHInterfaces ethernet_interfaces = {}

Definition at line 185 of file main.cpp.

◆ inv

struct inverters inv

◆ INV_CAN

FlexCAN_T4<CAN2, RX_SIZE_256, TX_SIZE_16> INV_CAN

Definition at line 168 of file main.cpp.

◆ load_cell_interface

LoadCellInterface load_cell_interface

Definition at line 199 of file main.cpp.

◆ logger

Definition at line 302 of file main.cpp.

◆ mcu_adc

Teensy_ADC<2> mcu_adc = Teensy_ADC<2>(DEFAULT_ANALOG_PINS)

Definition at line 179 of file main.cpp.

◆ pedals_system

◆ protobuf_recv_socket

EthernetUDP protobuf_recv_socket

Definition at line 165 of file main.cpp.

◆ protobuf_send_socket

EthernetUDP protobuf_send_socket

Definition at line 164 of file main.cpp.

◆ sab_interface

◆ Serial5

OrbisBR10 steering1& Serial5

Definition at line 180 of file main.cpp.

◆ simple_launch

Definition at line 313 of file main.cpp.

◆ steering1

SteeringSystem steering_system& steering1

Definition at line 216 of file main.cpp.

◆ sys_clock

SysClock sys_clock

Definition at line 215 of file main.cpp.

◆ tc_simple

TorqueControllerSimple tc_simple(1.0f, 1.0f) ( 1.  0f,
1.  0f 
)

◆ tc_vec

Definition at line 308 of file main.cpp.

◆ TELEM_CAN

FlexCAN_T4<CAN3, RX_SIZE_256, TX_SIZE_16> TELEM_CAN

Definition at line 169 of file main.cpp.

◆ telem_read_channels

const TelemetryInterfaceReadChannels telem_read_channels
Initial value:
= {
.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 int MCU15_CUR_POS_SENSE_CHANNEL
const int MCU15_FL_POTS_CHANNEL
const int MCU15_CUR_NEG_SENSE_CHANNEL
const int MCU15_FR_POTS_CHANNEL

Definition at line 57 of file main.cpp.

◆ torque_controller_mux

TCMuxType torque_controller_mux({static_cast< Controller * >(&tc_simple), static_cast< Controller * >(&tc_vec), static_cast< Controller * >(&case_wrapper), static_cast< Controller * >(&simple_launch), static_cast< Controller * >(&db_controller)}, {false, false, true, false, true}) ( {static_cast< Controller * >(&tc_simple), static_cast< Controller * >(&tc_vec), static_cast< Controller * >(&case_wrapper), static_cast< Controller * >(&simple_launch), static_cast< Controller * >(&db_controller)}  ,
{false, false, true, false, true}   
)

◆ wd_interface