From 15f288fd7c0acb92ff2487cc8fac14c783413642 Mon Sep 17 00:00:00 2001 From: helgie22 Date: Sun, 18 Oct 2026 14:30:08 +0000 Subject: [PATCH] =?UTF-8?q?Samskipti=20=C3=A1=20milli=20rpi=20og=20stm=20k?= =?UTF-8?q?ominn=20=C3=AD=20lag=20send=20can=20skilabo=C3=B0=20me=C3=B0=20?= =?UTF-8?q?filter?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/can_bus.h | 26 +++++++++ include/zephyr_can_bus.h | 29 ++++++++++ platformio.ini | 6 +-- src/app.cpp | 39 +++++++++++++- src/zephyr_can_bus.cpp | 112 +++++++++++++++++++++++++++++++++++++++ 5 files changed, 207 insertions(+), 5 deletions(-) create mode 100644 include/can_bus.h create mode 100644 include/zephyr_can_bus.h create mode 100644 src/zephyr_can_bus.cpp diff --git a/include/can_bus.h b/include/can_bus.h new file mode 100644 index 0000000..dc5e834 --- /dev/null +++ b/include/can_bus.h @@ -0,0 +1,26 @@ +#ifndef CAN_BUS_H +#define CAN_BUS_H + +#include + +// Rammategund CANopen-lagsins. Óháð Zephyr: engin dlc, engin flags. +// CANopen notar bara 11-bita staðlað ID +struct CanFrame +{ + uint16_t id; + uint8_t len; // Fjöldi bytes 0-8 + uint8_t data[8]; +}; + +// Það eina sem CANopen-lagið veit um CAN-businn +// Móttaka er ekki hér: main loop sækir ramma og gefur CanopenNode::process() +class CanBus +{ +public: + virtual ~CanBus() {} + + // Skilar false ef ekki tókst að setja rammann í sendibiðröð + virtual bool send(const CanFrame &frame) = 0; +}; + +#endif \ No newline at end of file diff --git a/include/zephyr_can_bus.h b/include/zephyr_can_bus.h new file mode 100644 index 0000000..5cd66a0 --- /dev/null +++ b/include/zephyr_can_bus.h @@ -0,0 +1,29 @@ +#ifndef ZEPHYR_CAN_BUS_H +#define ZEPHYR_CAN_BUS_H + +#include + +#include "can_bus.h" + +// Útfærsla á CanBus með Zephyr CAN-drivernum. +// Eini staðurinn þar sem CAN-rammar fara á milli Zephyr og okkar sniðs. + +class ZephyrCanBus : public CanBus +{ +private: + const struct device *dev; + +public: + ZephyrCanBus(); + + // Bitrate, síur og ræsing. Skilar 0 eða neikvæðum villukóða + int begin(); + + bool send(const CanFrame &frame) override; + + // Sækir einn ramma úr móttökubiðröðinni án þess að bíða + // Skilar true ef rammi var til. Ekki hluti af CanBus + bool receive(CanFrame &out); +}; + +#endif \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index e96be14..5605ada 100644 --- a/platformio.ini +++ b/platformio.ini @@ -36,6 +36,6 @@ upload_flags = ; CAN reception with a filter (src/can_rx_test.c) instead of the motor app. ; Build/upload: pio run -e can_rx_test -t upload -; [env:can_rx_test] -; extends = env:nucleo_l432kc -; build_flags = -D CAN_RX_TEST=1 +[env:can_rx_test] +extends = env:nucleo_l432kc +build_flags = -D CAN_RX_TEST=1 diff --git a/src/app.cpp b/src/app.cpp index 389aa8d..311f5c7 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -16,12 +16,16 @@ #include "operational_state.h" #include "stopped_state.h" -#include "node_application.h" +#include "zephyr_can_bus.h" + +// #include "node_application.h" óþarfi, er í motor_controller.h static RotaryEncoderDriver encoder; static MotorDriver motor; +static ZephyrCanBus can_bus; + const double Kp_P = 10.0; // P-controller from Project 2 const double Kp = 10.125; // Ziegler-Nichols PI @@ -96,16 +100,47 @@ extern "C" void motor_application_main(void) } motor.begin(); + + const int can_result = can_bus.begin(); + if (can_result < 0) + { + printk("CAN setup failed: %d\n", can_result); + return; + } + printk("CAN started\n"); + motor_controller.begin(); while (true) { char received_char; // Handle every character the ISR has queued since the last pass. - while (k_msgq_get(&rx_queue, &received_char, K_NO_WAIT) == 0) { + while (k_msgq_get(&rx_queue, &received_char, K_NO_WAIT) == 0) + { + if (received_char == 'b') + { + CanFrame boot = {}; + boot.id = 0x701; + boot.len = 1; + boot.data[0] = 0x00; + printk("CAN send 701: %s\n", can_bus.send(boot) ? "ok" : "FAILED"); + continue; + } + motor_controller.command(received_char); } + CanFrame frame; + while (can_bus.receive(frame)) + { + printk("CAN rx 0x%03X [%u]:", frame.id, frame.len); + for (uint8_t i = 0; i < frame.len; i++) + { + printk(" %02X", frame.data[i]); + } + printk("\n"); + } + motor_controller.update(); static NodeState last_state = NodeState::INITIALIZATION; diff --git a/src/zephyr_can_bus.cpp b/src/zephyr_can_bus.cpp new file mode 100644 index 0000000..692d2f2 --- /dev/null +++ b/src/zephyr_can_bus.cpp @@ -0,0 +1,112 @@ +#include "zephyr_can_bus.h" + +#include + +#include +#include + +#define CANBUS_NODE DT_CHOSEN(zephyr_canbus) + +static constexpr uint32_t CAN_BITRATE = 125000; // Verður að passa við can0 á rpi + +// COB-ID sem node-ið tekur við (Node ID 1) +static const uint16_t RX_IDS[] = { + 0x000, // NMT + 0x601, // SDO request + 0x201, // RPDO1 +}; + +// Móttökubiðröð fyrir allt að 10 ramma. Driverinn fyllir hana í interrupt, +// main loop tæmir hana. CAN_MSGQ_DEFINE býr til static global breytu, +// svo hún getyr ekki verið meðlimur í klasanum +CAN_MSGQ_DEFINE(can_rx_msgq, 10); + +// Keyrir í interrupt þegar rammi hefur verið sendur (eða sending mistókst). +// Gerir ekkert, en með callback er can_send() non-blocking: annars biði +// það eftir ACK, og án RPi á businum myndi main loop (og stýringin) stoppa. +static void tx_callback(const struct device *dev, int error, void *user_data) +{ + (void)dev; + (void)error; + (void)user_data; +} + +ZephyrCanBus::ZephyrCanBus() : dev(DEVICE_DT_GET(CANBUS_NODE)) +{ +} + +int ZephyrCanBus::begin() +{ + if (!device_is_ready(dev)) + { + return -ENODEV; + } + + int err = can_set_bitrate(dev, CAN_BITRATE); + if (err != 0) + { + return err; + } + + // Ein nákvæm sía fyrir hvert ID, allar í sömu biðröð + for (size_t i = 0; i < sizeof(RX_IDS) / sizeof(RX_IDS[0]); i++) + { + struct can_filter filter = {}; + filter.id = RX_IDS[i]; + filter.mask = CAN_STD_ID_MASK; // Öll 11 bitin verða að passa +#ifdef CAN_FILTER_DATA + filter.flags = CAN_FILTER_DATA; // Eldri Zephyr +#else + filter.flags = 0; +#endif + + const int filter_id = can_add_rx_filter_msgq(dev, &can_rx_msgq, &filter); + if (filter_id < 0) + { + return filter_id; + } + } + + return can_start(dev); +} + +bool ZephyrCanBus::send(const CanFrame &frame) +{ + if (frame.len > 8) + { + return false; + } + + // Núllstillt, annars gætu flag innihaldið rusl (extended ID, RTR o.s.frv.) + struct can_frame zf = {}; + zf.id = frame.id; + zf.dlc = can_bytes_to_dlc(frame.len); + memcpy(zf.data, frame.data, frame.len); + + // K_NO_WAIT: ef öll TX-hólfin eru full er rammanum sleppt frekar en að bíða + const int err = can_send(dev, &zf, K_NO_WAIT, tx_callback, nullptr); + + return err == 0; +} + +bool ZephyrCanBus::receive(CanFrame &out) +{ + struct can_frame zf; + + while (k_msgq_get(&can_rx_msgq, &zf, K_NO_WAIT) == 0) + { + // Hunsa RTR-ramma. CANopen notar þá ekki hér + if (zf.flags & CAN_FRAME_RTR) + { + continue; + } + + out.id = static_cast(zf.id); + out.len = can_dlc_to_bytes(zf.dlc); + memcpy(out.data, zf.data, out.len); + + return true; + } + + return false; +} \ No newline at end of file