diff --git a/include/InterfaceParams.h b/include/InterfaceParams.h index 0c55b6e92..0370d45f0 100644 --- a/include/InterfaceParams.h +++ b/include/InterfaceParams.h @@ -8,7 +8,7 @@ namespace EthParams {0x04, 0xe9, 0xe5, 0x10, 0x1f, 0x22}; const IPAddress default_MCU_ip(192, 168, 1, 30); - const IPAddress default_TCU_ip(192, 168, 1, 68); + const IPAddress default_TCU_ip(192, 168, 1, 69); const uint16_t default_protobuf_send_port = 2001; const uint16_t default_protobuf_recv_port = 2000; diff --git a/lib/interfaces/include/DrivebrainETHInterface.h b/lib/interfaces/include/DrivebrainETHInterface.h index 0ba8f0ea5..cc7e1c034 100644 --- a/lib/interfaces/include/DrivebrainETHInterface.h +++ b/lib/interfaces/include/DrivebrainETHInterface.h @@ -2,20 +2,19 @@ #define __DRIVEBRAINETHINTERFACE_H__ #include "hytech_msgs.pb.h" #include "DrivebrainData.h" - +#include "SharedDataTypes.h" class DrivebrainETHInterface { - public: - DrivebrainETHInterface() = default; - - void receive_pb_msg(const hytech_msgs_MCUData& msg_in); - - DrivebrainData_s get_latest_data() { return _latest_data; } +public: + DrivebrainETHInterface() = default; - private: + void receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis); - DrivebrainData_s _latest_data = {}; + hytech_msgs_MCUOutputData make_db_msg(const SharedCarState_s &shared_state); + DrivebrainData_s get_latest_data() { return _latest_data; } +private: + DrivebrainData_s _latest_data = {}; }; #endif // __DRIVEBRAINETHINTERFACE_H__ \ No newline at end of file diff --git a/lib/interfaces/include/ProtobufMsgInterface.h b/lib/interfaces/include/ProtobufMsgInterface.h index 61f2b36fc..328fdf6ed 100644 --- a/lib/interfaces/include/ProtobufMsgInterface.h +++ b/lib/interfaces/include/ProtobufMsgInterface.h @@ -13,21 +13,18 @@ struct ETHInterfaces { }; -// using recv_function_t = void (*)(const uint8_t* buffer, size_t packet_size, ETHInterfaces& interfaces); // this should be usable with arbitrary functions idk something template -void handle_ethernet_socket_receive(EthernetUDP *socket, std::function recv_function, eth_interface &interface, const pb_msgdesc_t *desc_pointer) +void handle_ethernet_socket_receive(const SysTick_s& tick, EthernetUDP *socket, std::function recv_function, eth_interface &interface, const pb_msgdesc_t *desc_pointer) { int packet_size = socket->parsePacket(); if (packet_size > 0) - { - Serial.println("packet size"); - Serial.println(packet_size); + { uint8_t buffer[buffer_size]; size_t read_bytes = socket->read(buffer, sizeof(buffer)); socket->read(buffer, UDP_TX_PACKET_MAX_SIZE); - recv_function(buffer, read_bytes, interface, desc_pointer); + recv_function(tick.millis, buffer, read_bytes, interface, desc_pointer); } } @@ -49,13 +46,13 @@ bool handle_ethernet_socket_send_pb(IPAddress addr, uint16_t port, EthernetUDP * } template -void recv_pb_stream_msg(const uint8_t *buffer, size_t packet_size, eth_interface &interface, const pb_msgdesc_t *desc_pointer) +void recv_pb_stream_msg(unsigned long curr_millis, const uint8_t *buffer, size_t packet_size, eth_interface &interface, const pb_msgdesc_t *desc_pointer) { pb_istream_t stream = pb_istream_from_buffer(buffer, packet_size); pb_msg_type msg = {}; if (pb_decode(&stream, desc_pointer, &msg)) { - interface.receive_pb_msg(msg); + interface.receive_pb_msg(msg, curr_millis); } } diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index fe4bf359e..030462577 100644 --- a/lib/interfaces/src/DrivebrainETHInterface.cpp +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -1,8 +1,31 @@ #include "DrivebrainETHInterface.h" - +#include "SharedDataTypes.h" #include -void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUData& msg_in) + +hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarState_s &shared_state) +{ + hytech_msgs_MCUOutputData out; + out.accel_percent = shared_state.pedals_data.accelPercent; + out.brake_percent = shared_state.pedals_data.brakePercent; + out.rpm_data = {shared_state.drivetrain_data.measuredSpeeds[0], + shared_state.drivetrain_data.measuredSpeeds[1], + shared_state.drivetrain_data.measuredSpeeds[2], + shared_state.drivetrain_data.measuredSpeeds[3]}; + return out; +} + +void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis) { - Serial.println("msg recvd"); - _latest_data = {}; + // Serial.println("msg recvd in here"); + veh_vec nm_lim; + nm_lim.construct(msg_in.torque_limit_nm.FL, msg_in.torque_limit_nm.FR, msg_in.torque_limit_nm.RL, msg_in.torque_limit_nm.RR); + + veh_vec speed_set; + speed_set.construct(msg_in.desired_rpms.FL, msg_in.desired_rpms.FR, msg_in.desired_rpms.RL, msg_in.desired_rpms.RR); + // Serial.println(msg_in.desired_rpms.FL); + + _latest_data.torque_limits_nm = nm_lim; + _latest_data.speed_setpoints_rpm = speed_set; + _latest_data.last_torque_lim_receive_time_millis = curr_millis; + _latest_data.last_speed_setpoint_receive_time_millis = curr_millis; } \ No newline at end of file diff --git a/lib/shared_data/include/Utility.h b/lib/shared_data/include/Utility.h index 1fbde62c9..66d0f2cb5 100644 --- a/lib/shared_data/include/Utility.h +++ b/lib/shared_data/include/Utility.h @@ -20,12 +20,12 @@ struct veh_vec T RL; T RR; - void construct(T FL, T FR, T RL, T RR) + void construct(T _FL, T _FR, T _RL, T _RR) { - FL = FL; - FR = FR; - RL = RL; - RR = RR; + FL = _FL; + FR = _FR; + RL = _RL; + RR = _RR; } /// @brief copy values to array in FL, FR, RL, RR order diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 320462eb4..834df4a74 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -1,7 +1,7 @@ #ifndef __DRIVEBRAINCONTROLLER_H__ #define __DRIVEBRAINCONTROLLER_H__ -#include "DrivebrainInterface.h" + #include "TorqueControllers.h" #include "DrivebrainData.h" #include "BaseController.h" diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index 3710f8b2b..122a00168 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -7,7 +7,7 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s & auto sys_tick = state.systick; auto db_input = state.db_data; - + bool speed_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_speed_setpoint_receive_time_millis)) > (int)_params.allowed_latency); bool torque_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_latency); diff --git a/platformio.ini b/platformio.ini index 5c535e22e..adc5e5ac3 100644 --- a/platformio.ini +++ b/platformio.ini @@ -4,7 +4,7 @@ lib_deps_shared = git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 https://github.com/hytech-racing/shared_firmware_types.git#feb3b83 https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 - https://github.com/hytech-racing/HT_proto/releases/download/2024-09-15T16_39_42/hytech_msgs_pb_lib.tar.gz + https://github.com/hytech-racing/HT_proto/releases/download/2024-09-17T03_08_13/hytech_msgs_pb_lib.tar.gz [env:test_env] platform = native diff --git a/src/main.cpp b/src/main.cpp index 0677dcdff..a4cf22553 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -341,7 +341,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick); /* Reset inverters */ void drivetrain_reset(); -void handle_ethernet_interface_comms(); +void handle_ethernet_interface_comms(const SysTick_s& systick, const hytech_msgs_MCUOutputData& out_msg); /* SETUP @@ -416,7 +416,6 @@ void loop() // get latest tick from sys clock SysTick_s curr_tick = sys_clock.tick(micros()); - handle_ethernet_interface_comms(); // process received CAN messages process_ring_buffer(CAN2_rxBuffer, CAN_receive_interfaces, curr_tick.millis); @@ -435,9 +434,13 @@ void loop() load_cell_interface.getLoadCellForces(), pedals_system.getPedalsSystemData(), vn_interface.get_vn_struct(), - db_interface.get_latest_db_data(), + db_eth_interface.get_latest_data(), torque_controller_mux.get_tc_mux_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 @@ -458,7 +461,8 @@ void loop() send_all_CAN_msgs(CAN3_txBuffer, &TELEM_CAN); // Basic debug prints - if (curr_tick.triggers.trigger5) + // if (curr_tick.triggers.trigger5) + if (false) { Serial.print("Steering system reported angle (deg): "); Serial.println(steering_system.getSteeringSystemData().angle); @@ -685,7 +689,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) } -void handle_ethernet_interface_comms() +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. @@ -693,7 +697,11 @@ void handle_ethernet_interface_comms() // Serial.println("bruh"); - std::function recv_boi = &recv_pb_stream_msg; - handle_ethernet_socket_receive<1024, hytech_msgs_MCUData>(&protobuf_recv_socket, recv_boi, db_eth_interface, hytech_msgs_MCUData_fields); + 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.trigger500) + { + 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