Updated can submodule

src/can.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <cstring>
+
 #include <esp_log.h>
 #include <driver/gpio.h>
 #include <driver/can.h>
@@ -12,11 +14,9 @@
 #include "globals.h"
 
 namespace {
-millis_t m_lastCanFeedback{};
-
 void initCan()
 {
-    Serial.printf("CAN info hello world\r\n");
+    Serial.println("initCan()");
 
     can_general_config_t g_config = CAN_GENERAL_CONFIG_DEFAULT(GPIO_NUM_21, GPIO_NUM_22, CAN_MODE_NORMAL);
     can_timing_config_t t_config CAN_TIMING_CONFIG_250KBITS();
@@ -59,100 +59,222 @@ void initCan()
     }
 }
 
-void parseCanInput()
+template<bool isBack>
+bool parseCanMessage(const can_message_t &message)
 {
-    can_message_t message;
+    auto &controller = isBack ? controllers.back : controllers.front;
-    if (const auto result = can_receive(&message, pdMS_TO_TICKS(200)); result != ESP_OK)
-    {
-        controllers.front.feedbackValid = millis() - m_lastCanFeedback <= 100;
-
-        if (result != ESP_ERR_TIMEOUT)
-            Serial.printf("CAN err can_receive() failed with %s\r\n", esp_err_to_name(result));
-        return;
-    }
 
     switch (message.identifier)
     {
-    case MotorControllerFrontLeftDcLink:
+    using namespace bobbycar::can;
-        m_lastCanFeedback = millis();
+    case MotorController<isBack, false>::Feedback::DcLink:
-        controllers.front.feedbackValid = true;
+        controller.feedback.left.current = *((int16_t*)message.data);
-        controllers.front.feedback.left.current = *((int16_t*)message.data);
+        return true;
-        break;
+    case MotorController<isBack, true>::Feedback::DcLink:
-    case MotorControllerFrontRightDcLink:
+        controller.feedback.right.current = *((int16_t*)message.data);
-        m_lastCanFeedback = millis();
+        return true;
-        controllers.front.feedbackValid = true;
+    case MotorController<isBack, false>::Feedback::Speed:
-        controllers.front.feedback.right.current = *((int16_t*)message.data);
+        controller.feedback.left.speed = *((int16_t*)message.data);
-        break;
+        return true;
-    case MotorControllerFrontLeftSpeed:
+    case MotorController<isBack, true>::Feedback::Speed:
-        m_lastCanFeedback = millis();
+        controller.feedback.right.speed = *((int16_t*)message.data);
-        controllers.front.feedbackValid = true;
+        return true;
-        controllers.front.feedback.left.speed = *((int16_t*)message.data);
+    case MotorController<isBack, false>::Feedback::Error:
-        break;
+        controller.feedback.left.error = *((int8_t*)message.data);
-    case MotorControllerFrontRightSpeed:
+        return true;
-        m_lastCanFeedback = millis();
+    case MotorController<isBack, true>::Feedback::Error:
-        controllers.front.feedbackValid = true;
+        controller.feedback.right.error = *((int8_t*)message.data);
-        controllers.front.feedback.right.speed = *((int16_t*)message.data);
+        return true;
-        break;
+    case MotorController<isBack, false>::Feedback::Angle:
-    case MotorControllerFrontLeftError:
+        controller.feedback.left.angle = *((int16_t*)message.data);
-        m_lastCanFeedback = millis();
+        return true;
-        controllers.front.feedbackValid = true;
+    case MotorController<isBack, true>::Feedback::Angle:
-        controllers.front.feedback.left.error = *((int8_t*)message.data);
+        controller.feedback.right.angle = *((int16_t*)message.data);
-        break;
+        return true;
-    case MotorControllerFrontRightError:
+    case MotorController<isBack, false>::Feedback::DcPhaA:
-        m_lastCanFeedback = millis();
+    case MotorController<isBack, true>::Feedback::DcPhaA:
-        controllers.front.feedbackValid = true;
+    case MotorController<isBack, false>::Feedback::DcPhaB:
-        controllers.front.feedback.right.error = *((int8_t*)message.data);
+    case MotorController<isBack, true>::Feedback::DcPhaB:
-        break;
+    case MotorController<isBack, false>::Feedback::DcPhaC:
-    case MotorControllerFrontLeftAngle:
+    case MotorController<isBack, true>::Feedback::DcPhaC:
-        m_lastCanFeedback = millis();
+        return true;
-        controllers.front.feedbackValid = true;
+    case MotorController<isBack, false>::Feedback::Chops:
-        controllers.front.feedback.left.angle = *((int16_t*)message.data);
+        controller.feedback.left.chops = *((uint16_t*)message.data);
-        break;
+        return true;
-    case MotorControllerFrontRightAngle:
+    case MotorController<isBack, true>::Feedback::Chops:
-        m_lastCanFeedback = millis();
+        controller.feedback.right.chops = *((uint16_t*)message.data);
-        controllers.front.feedbackValid = true;
+        return true;
-        controllers.front.feedback.right.angle = *((int16_t*)message.data);
+    case MotorController<isBack, false>::Feedback::Hall:
-        break;
+        controller.feedback.left.hallA = *((uint8_t*)message.data) & 1;
-    case MotorControllerFrontLeftChops:
+        controller.feedback.left.hallB = *((uint8_t*)message.data) & 2;
-        m_lastCanFeedback = millis();
+        controller.feedback.left.hallC = *((uint8_t*)message.data) & 4;
-        controllers.front.feedbackValid = true;
+        return true;
-        controllers.front.feedback.left.chops = *((uint16_t*)message.data);
+    case MotorController<isBack, true>::Feedback::Hall:
-        break;
+        controller.feedback.right.hallA = *((uint8_t*)message.data) & 1;
-    case MotorControllerFrontRightChops:
+        controller.feedback.right.hallB = *((uint8_t*)message.data) & 2;
-        m_lastCanFeedback = millis();
+        controller.feedback.right.hallC = *((uint8_t*)message.data) & 4;
-        controllers.front.feedbackValid = true;
+        return true;
-        controllers.front.feedback.right.chops = *((uint16_t*)message.data);
+    case MotorController<isBack, false>::Feedback::Voltage:
-        break;
+    case MotorController<isBack, true>::Feedback::Voltage:
-    case MotorControllerFrontLeftHall:
+        controller.feedback.batVoltage = *((int16_t*)message.data);
-    {
+        return true;
-        m_lastCanFeedback = millis();
+    case MotorController<isBack, false>::Feedback::Temp:
-        controllers.front.feedbackValid = true;
+    case MotorController<isBack, true>::Feedback::Temp:
-        controllers.front.feedback.left.hallA = *((uint8_t*)message.data) & 1;
+        controller.feedback.boardTemp = *((int16_t*)message.data);
-        controllers.front.feedback.left.hallB = *((uint8_t*)message.data) & 2;
+        return true;
-        controllers.front.feedback.left.hallC = *((uint8_t*)message.data) & 4;
-        break;
     }
-    case MotorControllerFrontRightHall:
+
+    return false;
+}
+
+bool parseCanInput()
+{
+    can_message_t message;
+    if (const auto result = can_receive(&message, pdMS_TO_TICKS(50)); result != ESP_OK)
     {
-        m_lastCanFeedback = millis();
+        if (millis() - controllers.front.lastCanFeedback > 100)
-        controllers.front.feedbackValid = true;
+            controllers.front.feedbackValid = false;
-        controllers.front.feedback.right.hallA = *((uint8_t*)message.data) & 1;
+
-        controllers.front.feedback.right.hallB = *((uint8_t*)message.data) & 2;
+        if (millis() - controllers.back.lastCanFeedback > 100)
-        controllers.front.feedback.right.hallC = *((uint8_t*)message.data) & 4;
+            controllers.back.feedbackValid = false;
-        break;
+
+        if (result != ESP_ERR_TIMEOUT)
+            Serial.printf("CAN err can_receive() failed with %s\r\n", esp_err_to_name(result));
+
+        return false;
     }
-    case MotorControllerFrontLeftVoltage:
+
-        m_lastCanFeedback = millis();
+    if (parseCanMessage<false>(message))
+    {
+        if (millis() - controllers.back.lastCanFeedback > 100)
+            controllers.back.feedbackValid = false;
+
+        controllers.front.lastCanFeedback = millis();
         controllers.front.feedbackValid = true;
-        controllers.front.feedback.batVoltage = *((int16_t*)message.data);
+        return true;
+    }
+    else
+    {
+        if (millis() - controllers.front.lastCanFeedback > 100)
+            controllers.front.feedbackValid = false;
+    }
+
+    if (parseCanMessage<true>(message))
+    {
+        controllers.back.lastCanFeedback = millis();
+        controllers.back.feedbackValid = true;
+        return true;
+    }
+    else
+    {
+        if (millis() - controllers.back.lastCanFeedback > 100)
+            controllers.back.feedbackValid = false;
+    }
+
+    Serial.printf("WARNING Unknown CAN info received .identifier = %u\r\n", message.identifier);
+
+    return true;
+}
+
+void sendCanCommands()
+{
+    constexpr auto send = [](uint32_t addr, auto value){
+        can_message_t message;
+        message.identifier = addr;
+        message.flags = CAN_MSG_FLAG_SS;
+        message.data_length_code = sizeof(value);
+        std::fill(std::begin(message.data), std::end(message.data), 0);
+        std::memcpy(message.data, &value, sizeof(value));
+
+        const auto result = can_transmit(&message, pdMS_TO_TICKS(200));
+        if (result != ESP_OK && result != ESP_ERR_TIMEOUT)
+            Serial.printf("ERROR: can_transmit() failed with %s\r\n", esp_err_to_name(result));
+        return result;
+    };
+
+    using namespace bobbycar::can;
+
+    send(MotorController<false, false>::Command::InpTgt, controllers.front.command.left.pwm);
+    send(MotorController<false, true>::Command::InpTgt, controllers.front.command.right.pwm);
+    send(MotorController<true, false>::Command::InpTgt, controllers.back.command.left.pwm);
+    send(MotorController<true, true>::Command::InpTgt, controllers.back.command.right.pwm);
+
+    static int i{};
+    switch (i++)
+    {
+    case 0:
+        send(MotorController<false, false>::Command::Enable, controllers.front.command.left.enable);
+        send(MotorController<false, true>::Command::Enable, controllers.front.command.right.enable);
+        send(MotorController<true, false>::Command::Enable, controllers.back.command.left.enable);
+        send(MotorController<true, true>::Command::Enable, controllers.back.command.right.enable);
         break;
-    case MotorControllerFrontLeftTemp:
+    case 1:
-        m_lastCanFeedback = millis();
+        send(MotorController<false, false>::Command::CtrlTyp, controllers.front.command.left.ctrlTyp);
-        controllers.front.feedbackValid = true;
+        send(MotorController<false, true>::Command::CtrlTyp, controllers.front.command.right.ctrlTyp);
-        controllers.front.feedback.boardTemp = *((int16_t*)message.data);
+        send(MotorController<true, false>::Command::CtrlTyp, controllers.back.command.left.ctrlTyp);
+        send(MotorController<true, true>::Command::CtrlTyp, controllers.back.command.right.ctrlTyp);
+        break;
+    case 2:
+        send(MotorController<false, false>::Command::CtrlMod, controllers.front.command.left.ctrlMod);
+        send(MotorController<false, true>::Command::CtrlMod, controllers.front.command.right.ctrlMod);
+        send(MotorController<true, false>::Command::CtrlMod, controllers.back.command.left.ctrlMod);
+        send(MotorController<true, true>::Command::CtrlMod, controllers.back.command.right.ctrlMod);
+        break;
+    case 3:
+        send(MotorController<false, false>::Command::IMotMax, controllers.front.command.left.iMotMax);
+        send(MotorController<false, true>::Command::IMotMax, controllers.front.command.right.iMotMax);
+        send(MotorController<true, false>::Command::IMotMax, controllers.back.command.left.iMotMax);
+        send(MotorController<true, true>::Command::IMotMax, controllers.back.command.right.iMotMax);
+        break;
+    case 4:
+        send(MotorController<false, false>::Command::IDcMax, controllers.front.command.left.iDcMax);
+        send(MotorController<false, true>::Command::IDcMax, controllers.front.command.right.iDcMax);
+        send(MotorController<true, false>::Command::IDcMax, controllers.back.command.left.iDcMax);
+        send(MotorController<true, true>::Command::IDcMax, controllers.back.command.right.iDcMax);
+        break;
+    case 5:
+        send(MotorController<false, false>::Command::NMotMax, controllers.front.command.left.nMotMax);
+        send(MotorController<false, true>::Command::NMotMax, controllers.front.command.right.nMotMax);
+        send(MotorController<true, false>::Command::NMotMax, controllers.back.command.left.nMotMax);
+        send(MotorController<true, true>::Command::NMotMax, controllers.back.command.right.nMotMax);
+        break;
+    case 6:
+        send(MotorController<false, false>::Command::FieldWeakMax, controllers.front.command.left.fieldWeakMax);
+        send(MotorController<false, true>::Command::FieldWeakMax, controllers.front.command.right.fieldWeakMax);
+        send(MotorController<true, false>::Command::FieldWeakMax, controllers.back.command.left.fieldWeakMax);
+        send(MotorController<true, true>::Command::FieldWeakMax, controllers.back.command.right.fieldWeakMax);
+        break;
+    case 7:
+        send(MotorController<false, false>::Command::PhaseAdvMax, controllers.front.command.left.phaseAdvMax);
+        send(MotorController<false, true>::Command::PhaseAdvMax, controllers.front.command.right.phaseAdvMax);
+        send(MotorController<true, false>::Command::PhaseAdvMax, controllers.back.command.left.phaseAdvMax);
+        send(MotorController<true, true>::Command::PhaseAdvMax, controllers.back.command.right.phaseAdvMax);
+        break;
+    case 8:
+        send(MotorController<false, false>::Command::BuzzerFreq, controllers.front.command.buzzer.freq);
+        send(MotorController<false, true>::Command::BuzzerFreq, controllers.front.command.buzzer.freq);
+        send(MotorController<true, false>::Command::BuzzerFreq, controllers.back.command.buzzer.freq);
+        send(MotorController<true, true>::Command::BuzzerFreq, controllers.back.command.buzzer.freq);
+        break;
+    case 9:
+        send(MotorController<false, false>::Command::BuzzerPattern, controllers.front.command.buzzer.pattern);
+        send(MotorController<false, true>::Command::BuzzerPattern, controllers.front.command.buzzer.pattern);
+        send(MotorController<true, false>::Command::BuzzerPattern, controllers.back.command.buzzer.pattern);
+        send(MotorController<true, true>::Command::BuzzerPattern, controllers.back.command.buzzer.pattern);
+        break;
+    case 10:
+        send(MotorController<false, false>::Command::Led, controllers.front.command.led);
+        send(MotorController<false, true>::Command::Led, controllers.front.command.led);
+        send(MotorController<true, false>::Command::Led, controllers.back.command.led);
+        send(MotorController<true, true>::Command::Led, controllers.back.command.led);
+        break;
+    case 11:
+        send(MotorController<false, false>::Command::Poweroff, controllers.front.command.poweroff);
+        send(MotorController<false, true>::Command::Poweroff, controllers.front.command.poweroff);
+        send(MotorController<true, false>::Command::Poweroff, controllers.back.command.poweroff);
+        send(MotorController<true, true>::Command::Poweroff, controllers.back.command.poweroff);
+        i=0;
         break;
-    default:
-        Serial.printf("WARNING Unknown CAN info received .identifier = %u\r\n", message.identifier);
     }
 }
 }

src/controller.h

@@ -5,6 +5,8 @@
 #include "bobbycar-protocol/bobbycar-common.h"
 #include "bobbycar-protocol/bobbycar-serial.h"
 
+#include "types.h"
+
 #ifndef FEATURE_CAN
 #include "feedbackparser.h"
 #endif
@@ -36,6 +38,9 @@ struct Controller {
 
     Command command{};
 
+#ifdef FEATURE_CAN
+    millis_t lastCanFeedback{};
+#endif
     bool feedbackValid{};
     Feedback feedback{};
 

src/main.cpp

@@ -429,7 +429,9 @@ void loop()
     }
 
 #ifdef FEATURE_CAN
-    parseCanInput();
+    for (int i = 0; i < 4; i++)
+        if (!parseCanInput())
+            break;
 #else
     for (Controller &controller : controllers)
         controller.parser.update();

src/utils.h

@@ -2,7 +2,6 @@
 
 #include <algorithm>
 #include <utility>
-#include <cstring>
 
 #include <driver/can.h>
 
@@ -14,6 +13,9 @@
 
 #include "display.h"
 #include "globals.h"
+#ifdef FEATURE_CAN
+#include "can.h"
+#endif
 
 // macros are a shit piece of software
 #define STRING2(s) #s
@@ -259,108 +261,8 @@ void sendCommands()
         controller.command.checksum = calculateChecksum(controller.command);
         controller.serial.get().write((uint8_t *) &controller.command, sizeof(controller.command));
     }
-#endif
+#else
-
+    sendCanCommands();
-#ifdef FEATURE_CAN
-    constexpr auto send = [](uint32_t addr, auto value){
-        can_message_t message;
-        message.identifier = addr;
-        if (addr == MotorControllerFrontLeftInpTgt || addr == MotorControllerFrontRightInpTgt)
-            message.flags = CAN_MSG_FLAG_SS;
-        else
-            message.flags = CAN_MSG_FLAG_NONE;
-        message.data_length_code = sizeof(value);
-        std::fill(std::begin(message.data), std::end(message.data), 0);
-        std::memcpy(message.data, &value, sizeof(value));
-
-        const auto result = can_transmit(&message, pdMS_TO_TICKS(200));
-        if (result != ESP_OK && result != ESP_ERR_TIMEOUT)
-            Serial.printf("ERROR: can_transmit() failed with %s\r\n", esp_err_to_name(result));
-        return result;
-    };
-
-    send(MotorControllerFrontLeftInpTgt, controllers.front.command.left.pwm);
-    send(MotorControllerFrontRightInpTgt, controllers.front.command.right.pwm);
-//    send(MotorControllerBackLeftInpTgt, controllers.back.command.left.pwm);
-//    send(MotorControllerBackRightInpTgt, controllers.back.command.right.pwm);
-
-    static int i = 0;
-    switch (i++)
-    {
-    case 0:
-    send(MotorControllerFrontLeftEnable, controllers.front.command.left.enable);
-    send(MotorControllerFrontRightEnable, controllers.front.command.right.enable);
-//    send(MotorControllerBackLeftEnable, controllers.back.command.left.enable);
-//    send(MotorControllerBackRightEnable, controllers.back.command.right.enable);
-    break;
-    case 1:
-    send(MotorControllerFrontLeftCtrlTyp, controllers.front.command.left.ctrlTyp);
-    send(MotorControllerFrontRightCtrlTyp, controllers.front.command.right.ctrlTyp);
-//    send(MotorControllerBackLeftCtrlTyp, controllers.back.command.left.ctrlTyp);
-//    send(MotorControllerBackRightCtrlTyp, controllers.back.command.right.ctrlTyp);
-    break;
-    case 2:
-    send(MotorControllerFrontLeftCtrlMod, controllers.front.command.left.ctrlMod);
-    send(MotorControllerFrontRightCtrlMod, controllers.front.command.right.ctrlMod);
-//    send(MotorControllerBackLeftCtrlMod, controllers.back.command.left.ctrlMod);
-//    send(MotorControllerBackRightCtrlMod, controllers.back.command.right.ctrlMod);
-    break;
-    case 3:
-    send(MotorControllerFrontLeftIMotMax, controllers.front.command.left.iMotMax);
-    send(MotorControllerFrontRightIMotMax, controllers.front.command.right.iMotMax);
-//    send(MotorControllerBackLeftIMotMax, controllers.back.command.left.iMotMax);
-//    send(MotorControllerBackRightIMotMax, controllers.back.command.right.iMotMax);
-    break;
-    case 4:
-    send(MotorControllerFrontLeftIDcMax, controllers.front.command.left.iDcMax);
-    send(MotorControllerFrontRightIDcMax, controllers.front.command.right.iDcMax);
-//    send(MotorControllerBackLeftIDcMax, controllers.back.command.left.iDcMax);
-//    send(MotorControllerBackRightIDcMax, controllers.back.command.right.iDcMax);
-    break;
-    case 5:
-    send(MotorControllerFrontLeftNMotMax, controllers.front.command.left.nMotMax);
-    send(MotorControllerFrontRightNMotMax, controllers.front.command.right.nMotMax);
-//    send(MotorControllerBackLeftNMotMax, controllers.back.command.left.nMotMax);
-//    send(MotorControllerBackRightNMotMax, controllers.back.command.right.nMotMax);
-    break;
-    case 6:
-    send(MotorControllerFrontLeftFieldWeakMax, controllers.front.command.left.fieldWeakMax);
-    send(MotorControllerFrontRightFieldWeakMax, controllers.front.command.right.fieldWeakMax);
-//    send(MotorControllerBackLeftFieldWeakMax, controllers.back.command.left.fieldWeakMax);
-//    send(MotorControllerBackRightFieldWeakMax, controllers.back.command.right.fieldWeakMax);
-    break;
-    case 7:
-    send(MotorControllerFrontLeftPhaseAdvMax, controllers.front.command.left.phaseAdvMax);
-    send(MotorControllerFrontRightPhaseAdvMax, controllers.front.command.right.phaseAdvMax);
-//    send(MotorControllerBackLeftPhaseAdvMax, controllers.back.command.left.phaseAdvMax);
-//    send(MotorControllerBackRightPhaseAdvMax, controllers.back.command.right.phaseAdvMax);
-    break;
-    case 8:
-    send(MotorControllerFrontLeftBuzzerFreq, controllers.front.command.buzzer.freq);
-    send(MotorControllerFrontRightBuzzerFreq, controllers.front.command.buzzer.freq);
-//    send(MotorControllerBackLeftBuzzerFreq, controllers.back.command.buzzer.freq);
-//    send(MotorControllerBackRightBuzzerFreq, controllers.back.command.buzzer.freq);
-    break;
-    case 9:
-    send(MotorControllerFrontLeftBuzzerPattern, controllers.front.command.buzzer.pattern);
-    send(MotorControllerFrontRightBuzzerPattern, controllers.front.command.buzzer.pattern);
-//    send(MotorControllerBackLeftBuzzerPattern, controllers.back.command.buzzer.pattern);
-//    send(MotorControllerBackRightBuzzerPattern, controllers.back.command.buzzer.pattern);
-    break;
-    case 10:
-    send(MotorControllerFrontLeftLed, controllers.front.command.led);
-    send(MotorControllerFrontRightLed, controllers.front.command.led);
-//    send(MotorControllerBackLeftLed, controllers.back.command.led);
-//    send(MotorControllerBackRightLed, controllers.back.command.led);
-    break;
-    case 11:
-    send(MotorControllerFrontLeftPoweroff, controllers.front.command.poweroff);
-    send(MotorControllerFrontRightPoweroff, controllers.front.command.poweroff);
-//    send(MotorControllerBackLeftPoweroff, controllers.back.command.poweroff);
-//    send(MotorControllerBackRightPoweroff, controllers.back.command.poweroff);
-    i=0;
-    break;
-    }
 #endif
 }