Moved sources into separate .cpp files
This commit is contained in:
455
main/can.cpp
455
main/can.cpp
@@ -0,0 +1,455 @@
|
||||
#include "can.h"
|
||||
|
||||
// system includes
|
||||
#include <cstring>
|
||||
#include <optional>
|
||||
|
||||
// esp-idf
|
||||
#include <driver/gpio.h>
|
||||
#include <driver/twai.h>
|
||||
#include <esp_log.h>
|
||||
|
||||
// 3rdparty lib includes
|
||||
#include <espchrono.h>
|
||||
#include <tickchrono.h>
|
||||
|
||||
// local includes
|
||||
#include "bobbycar-can.h"
|
||||
#include "globals.h"
|
||||
#include "buttons.h"
|
||||
|
||||
namespace can {
|
||||
namespace {
|
||||
constexpr const char * const TAG = "BOBBYCAN";
|
||||
} // namespace
|
||||
|
||||
std::optional<int16_t> can_gas, can_brems;
|
||||
espchrono::millis_clock::time_point last_can_gas{}, last_can_brems{};
|
||||
CanButtonsState lastButtonsState;
|
||||
|
||||
void initCan()
|
||||
{
|
||||
ESP_LOGI(TAG, "called");
|
||||
|
||||
twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(GPIO_NUM_21, GPIO_NUM_22, TWAI_MODE_NORMAL);
|
||||
twai_timing_config_t t_config TWAI_TIMING_CONFIG_250KBITS();
|
||||
twai_filter_config_t f_config TWAI_FILTER_CONFIG_ACCEPT_ALL();
|
||||
// {
|
||||
// //
|
||||
// .acceptance_code = 0b00000000000,
|
||||
// .acceptance_mask = 0b00001111111,
|
||||
// .single_filter = true
|
||||
// };
|
||||
|
||||
if (const auto result = twai_driver_install(&g_config, &t_config, &f_config); result == ESP_OK)
|
||||
{
|
||||
ESP_LOGI(TAG, "twai_driver_install() succeeded");
|
||||
}
|
||||
else
|
||||
{
|
||||
ESP_LOGE(TAG, "twai_driver_install() failed with %s", esp_err_to_name(result));
|
||||
return;
|
||||
}
|
||||
|
||||
if (const auto result = twai_start(); result == ESP_OK)
|
||||
{
|
||||
ESP_LOGI(TAG, "twai_start() succeeded");
|
||||
}
|
||||
else
|
||||
{
|
||||
ESP_LOGE(TAG, "twai_start() failed with %s", esp_err_to_name(result));
|
||||
|
||||
if (const auto result = twai_driver_uninstall(); result == ESP_OK)
|
||||
{
|
||||
ESP_LOGI(TAG, "twai_driver_uninstall() succeeded");
|
||||
}
|
||||
else
|
||||
{
|
||||
ESP_LOGE(TAG, "twai_driver_uninstall() failed with %s", esp_err_to_name(result));
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
template<bool isBack>
|
||||
bool parseMotorControllerCanMessage(const twai_message_t &message, Controller &controller)
|
||||
{
|
||||
switch (message.identifier)
|
||||
{
|
||||
using namespace bobbycar::protocol::can;
|
||||
case MotorController<isBack, false>::Feedback::DcLink:
|
||||
controller.feedback.left.dcLink = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, true>::Feedback::DcLink:
|
||||
controller.feedback.right.dcLink = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, false>::Feedback::Speed:
|
||||
controller.feedback.left.speed = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, true>::Feedback::Speed:
|
||||
controller.feedback.right.speed = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, false>::Feedback::Error:
|
||||
controller.feedback.left.error = *((int8_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, true>::Feedback::Error:
|
||||
controller.feedback.right.error = *((int8_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, false>::Feedback::Angle:
|
||||
controller.feedback.left.angle = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, true>::Feedback::Angle:
|
||||
controller.feedback.right.angle = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, false>::Feedback::DcPhaA:
|
||||
controller.feedback.left.dcPhaA = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, true>::Feedback::DcPhaA:
|
||||
controller.feedback.right.dcPhaA = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, false>::Feedback::DcPhaB:
|
||||
controller.feedback.left.dcPhaB = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, true>::Feedback::DcPhaB:
|
||||
controller.feedback.right.dcPhaB = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, false>::Feedback::DcPhaC:
|
||||
controller.feedback.left.dcPhaC = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, true>::Feedback::DcPhaC:
|
||||
controller.feedback.right.dcPhaC = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, false>::Feedback::Chops:
|
||||
controller.feedback.left.chops = *((uint16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, true>::Feedback::Chops:
|
||||
controller.feedback.right.chops = *((uint16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, false>::Feedback::Hall:
|
||||
controller.feedback.left.hallA = *((uint8_t*)message.data) & 1;
|
||||
controller.feedback.left.hallB = *((uint8_t*)message.data) & 2;
|
||||
controller.feedback.left.hallC = *((uint8_t*)message.data) & 4;
|
||||
return true;
|
||||
case MotorController<isBack, true>::Feedback::Hall:
|
||||
controller.feedback.right.hallA = *((uint8_t*)message.data) & 1;
|
||||
controller.feedback.right.hallB = *((uint8_t*)message.data) & 2;
|
||||
controller.feedback.right.hallC = *((uint8_t*)message.data) & 4;
|
||||
return true;
|
||||
case MotorController<isBack, false>::Feedback::Voltage:
|
||||
case MotorController<isBack, true>::Feedback::Voltage:
|
||||
controller.feedback.batVoltage = *((int16_t*)message.data);
|
||||
return true;
|
||||
case MotorController<isBack, false>::Feedback::Temp:
|
||||
case MotorController<isBack, true>::Feedback::Temp:
|
||||
controller.feedback.boardTemp = *((int16_t*)message.data);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool parseBoardcomputerCanMessage(const twai_message_t &message)
|
||||
{
|
||||
switch (message.identifier)
|
||||
{
|
||||
using namespace bobbycar::protocol::can;
|
||||
case Boardcomputer::Command::ButtonPress:
|
||||
{
|
||||
const auto canButtonBits = *((uint16_t*)message.data);
|
||||
CanButtonsState newState {
|
||||
.up = bool(canButtonBits & Boardcomputer::ButtonUp),
|
||||
.down = bool(canButtonBits & Boardcomputer::ButtonDown),
|
||||
.confirm = bool(canButtonBits & Boardcomputer::ButtonConfirm),
|
||||
.back = bool(canButtonBits & Boardcomputer::ButtonBack),
|
||||
.profile0 = bool(canButtonBits & Boardcomputer::ButtonProfile0),
|
||||
.profile1 = bool(canButtonBits & Boardcomputer::ButtonProfile1),
|
||||
.profile2 = bool(canButtonBits & Boardcomputer::ButtonProfile2),
|
||||
.profile3 = bool(canButtonBits & Boardcomputer::ButtonProfile3),
|
||||
};
|
||||
|
||||
if (lastButtonsState.up != newState.up)
|
||||
InputDispatcher::upButton(newState.up);
|
||||
|
||||
if (lastButtonsState.down != newState.down)
|
||||
InputDispatcher::downButton(newState.down);
|
||||
|
||||
if (lastButtonsState.confirm != newState.confirm)
|
||||
InputDispatcher::confirmButton(newState.confirm);
|
||||
|
||||
if (lastButtonsState.back != newState.back)
|
||||
InputDispatcher::backButton(newState.back);
|
||||
|
||||
if (lastButtonsState.profile0 != newState.profile0)
|
||||
InputDispatcher::profileButton(0, newState.profile0);
|
||||
|
||||
if (lastButtonsState.profile1 != newState.profile1)
|
||||
InputDispatcher::profileButton(1, newState.profile1);
|
||||
|
||||
if (lastButtonsState.profile2 != newState.profile2)
|
||||
InputDispatcher::profileButton(2, newState.profile2);
|
||||
|
||||
if (lastButtonsState.profile3 != newState.profile3)
|
||||
InputDispatcher::profileButton(3, newState.profile3);
|
||||
|
||||
lastButtonsState = newState;
|
||||
break;
|
||||
}
|
||||
case Boardcomputer::Command::RawGas:
|
||||
can_gas = *((int16_t*)message.data);
|
||||
last_can_gas = espchrono::millis_clock::now();
|
||||
break;
|
||||
case Boardcomputer::Command::RawBrems:
|
||||
can_brems = *((int16_t*)message.data);
|
||||
last_can_brems = espchrono::millis_clock::now();
|
||||
break;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
bool tryParseCanInput()
|
||||
{
|
||||
twai_message_t message;
|
||||
const auto timeout = std::chrono::ceil<espcpputils::ticks>(espchrono::milliseconds32{settings.controllerHardware.canReceiveTimeout}).count();
|
||||
if (const auto result = twai_receive(&message, timeout); result != ESP_OK)
|
||||
{
|
||||
if (result != ESP_ERR_TIMEOUT)
|
||||
{
|
||||
ESP_LOGE(TAG, "twai_receive() failed with %s", esp_err_to_name(result));
|
||||
}
|
||||
|
||||
if (espchrono::millis_clock::now() - controllers.front.lastCanFeedback > 100ms)
|
||||
controllers.front.feedbackValid = false;
|
||||
|
||||
if (espchrono::millis_clock::now() - controllers.back.lastCanFeedback > 100ms)
|
||||
controllers.back.feedbackValid = false;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
Controller &front = settings.controllerHardware.swapFrontBack ? controllers.back : controllers.front;
|
||||
Controller &back = settings.controllerHardware.swapFrontBack ? controllers.front : controllers.back;
|
||||
|
||||
if (parseMotorControllerCanMessage<false>(message, front))
|
||||
{
|
||||
if (espchrono::millis_clock::now() - back.lastCanFeedback > 100ms)
|
||||
back.feedbackValid = false;
|
||||
|
||||
front.lastCanFeedback = espchrono::millis_clock::now();
|
||||
front.feedbackValid = true;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (espchrono::millis_clock::now() - front.lastCanFeedback > 100ms)
|
||||
front.feedbackValid = false;
|
||||
}
|
||||
|
||||
if (parseMotorControllerCanMessage<true>(message, back))
|
||||
{
|
||||
back.lastCanFeedback = espchrono::millis_clock::now();
|
||||
back.feedbackValid = true;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (espchrono::millis_clock::now() - back.lastCanFeedback > 100ms)
|
||||
back.feedbackValid = false;
|
||||
}
|
||||
|
||||
if (parseBoardcomputerCanMessage(message))
|
||||
return true;
|
||||
|
||||
ESP_LOGW(TAG, "Unknown CAN info received .identifier = %u", message.identifier);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void parseCanInput()
|
||||
{
|
||||
for (int i = 0; i < 4; i++)
|
||||
if (!tryParseCanInput())
|
||||
break;
|
||||
}
|
||||
|
||||
void sendCanCommands()
|
||||
{
|
||||
constexpr auto send = [](uint32_t addr, auto value){
|
||||
twai_message_t message;
|
||||
message.identifier = addr;
|
||||
message.flags = TWAI_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 timeout = std::chrono::ceil<espcpputils::ticks>(espchrono::milliseconds32{settings.controllerHardware.canTransmitTimeout}).count();
|
||||
const auto result = twai_transmit(&message, timeout);
|
||||
if (result != ESP_OK && result != ESP_ERR_TIMEOUT)
|
||||
{
|
||||
ESP_LOGE(TAG, "ERROR: twai_transmit() failed with %s", esp_err_to_name(result));
|
||||
}
|
||||
return result;
|
||||
};
|
||||
|
||||
const bool swap = settings.controllerHardware.swapFrontBack;
|
||||
const Controller *front =
|
||||
(swap ? settings.controllerHardware.sendBackCanCmd : settings.controllerHardware.sendFrontCanCmd ) ?
|
||||
(swap ? &controllers.back : &controllers.front) :
|
||||
nullptr;
|
||||
const Controller *back =
|
||||
(swap ? settings.controllerHardware.sendFrontCanCmd : settings.controllerHardware.sendBackCanCmd ) ?
|
||||
(swap ? &controllers.front : &controllers.back) :
|
||||
nullptr;
|
||||
|
||||
using namespace bobbycar::protocol::can;
|
||||
|
||||
if (front) send(MotorController<false, false>::Command::InpTgt, front->command.left.pwm);
|
||||
if (front) send(MotorController<false, true>::Command::InpTgt, front->command.right.pwm);
|
||||
if (back) send(MotorController<true, false>::Command::InpTgt, back->command.left.pwm);
|
||||
if (back) send(MotorController<true, true>::Command::InpTgt, back->command.right.pwm);
|
||||
|
||||
uint16_t buttonLeds{};
|
||||
if (const auto index = settingsPersister.currentlyOpenProfileIndex())
|
||||
switch (*index)
|
||||
{
|
||||
case 0: buttonLeds |= Boardcomputer::ButtonProfile0; break;
|
||||
case 1: buttonLeds |= Boardcomputer::ButtonProfile1; break;
|
||||
case 2: buttonLeds |= Boardcomputer::ButtonProfile2; break;
|
||||
case 3: buttonLeds |= Boardcomputer::ButtonProfile3; break;
|
||||
}
|
||||
|
||||
static struct {
|
||||
struct {
|
||||
uint8_t freq = 0;
|
||||
uint8_t pattern = 0;
|
||||
} front, back;
|
||||
uint16_t buttonLeds{};
|
||||
} lastValues;
|
||||
|
||||
static int i{};
|
||||
|
||||
if ((front && front->command.buzzer.freq != lastValues.front.freq ) ||
|
||||
(front && front->command.buzzer.pattern != lastValues.front.pattern ) ||
|
||||
(back && back->command.buzzer.freq != lastValues.back.freq) ||
|
||||
(back && back->command.buzzer.pattern != lastValues.back.pattern))
|
||||
i = 10;
|
||||
else if (buttonLeds != lastValues.buttonLeds)
|
||||
i = 12;
|
||||
|
||||
switch (i++)
|
||||
{
|
||||
case 0:
|
||||
if (front) send(MotorController<false, false>::Command::Enable, front->command.left.enable);
|
||||
if (front) send(MotorController<false, true>::Command::Enable, front->command.right.enable);
|
||||
if (back) send(MotorController<true, false>::Command::Enable, back->command.left.enable);
|
||||
if (back) send(MotorController<true, true>::Command::Enable, back->command.right.enable);
|
||||
break;
|
||||
case 1:
|
||||
if (front) send(MotorController<false, false>::Command::CtrlTyp, front->command.left.ctrlTyp);
|
||||
if (front) send(MotorController<false, true>::Command::CtrlTyp, front->command.right.ctrlTyp);
|
||||
if (back) send(MotorController<true, false>::Command::CtrlTyp, back->command.left.ctrlTyp);
|
||||
if (back) send(MotorController<true, true>::Command::CtrlTyp, back->command.right.ctrlTyp);
|
||||
break;
|
||||
case 2:
|
||||
if (front) send(MotorController<false, false>::Command::CtrlMod, front->command.left.ctrlMod);
|
||||
if (front) send(MotorController<false, true>::Command::CtrlMod, front->command.right.ctrlMod);
|
||||
if (back) send(MotorController<true, false>::Command::CtrlMod, back->command.left.ctrlMod);
|
||||
if (back) send(MotorController<true, true>::Command::CtrlMod, back->command.right.ctrlMod);
|
||||
break;
|
||||
case 3:
|
||||
#if defined(HAS_SIMPLIFIED)
|
||||
SIMPLIFIED_IMOTMAX
|
||||
#endif
|
||||
if (front) send(MotorController<false, false>::Command::IMotMax, front->command.left.iMotMax);
|
||||
if (front) send(MotorController<false, true>::Command::IMotMax, front->command.right.iMotMax);
|
||||
if (back) send(MotorController<true, false>::Command::IMotMax, back->command.left.iMotMax);
|
||||
if (back) send(MotorController<true, true>::Command::IMotMax, back->command.right.iMotMax);
|
||||
break;
|
||||
case 4:
|
||||
#if defined(HAS_SIMPLIFIED)
|
||||
SIMPLIFIED_IDCMAX
|
||||
#endif
|
||||
if (front) send(MotorController<false, false>::Command::IDcMax, front->command.left.iDcMax);
|
||||
if (front) send(MotorController<false, true>::Command::IDcMax, front->command.right.iDcMax);
|
||||
if (back) send(MotorController<true, false>::Command::IDcMax, back->command.left.iDcMax);
|
||||
if (back) send(MotorController<true, true>::Command::IDcMax, back->command.right.iDcMax);
|
||||
break;
|
||||
case 5:
|
||||
#if defined(HAS_SIMPLIFIED)
|
||||
SIMPLIFIED_NMOTMAX
|
||||
#endif
|
||||
if (front) send(MotorController<false, false>::Command::NMotMax, front->command.left.nMotMax);
|
||||
if (front) send(MotorController<false, true>::Command::NMotMax, front->command.right.nMotMax);
|
||||
if (back) send(MotorController<true, false>::Command::NMotMax, back->command.left.nMotMax);
|
||||
if (back) send(MotorController<true, true>::Command::NMotMax, back->command.right.nMotMax);
|
||||
break;
|
||||
case 6:
|
||||
#if defined(HAS_SIMPLIFIED)
|
||||
SIMPLIFIED_FIELDWEAKMAX
|
||||
#endif
|
||||
if (front) send(MotorController<false, false>::Command::FieldWeakMax, front->command.left.fieldWeakMax);
|
||||
if (front) send(MotorController<false, true>::Command::FieldWeakMax, front->command.right.fieldWeakMax);
|
||||
if (back) send(MotorController<true, false>::Command::FieldWeakMax, back->command.left.fieldWeakMax);
|
||||
if (back) send(MotorController<true, true>::Command::FieldWeakMax, back->command.right.fieldWeakMax);
|
||||
break;
|
||||
case 7:
|
||||
if (front) send(MotorController<false, false>::Command::PhaseAdvMax, front->command.left.phaseAdvMax);
|
||||
if (front) send(MotorController<false, true>::Command::PhaseAdvMax, front->command.right.phaseAdvMax);
|
||||
if (back) send(MotorController<true, false>::Command::PhaseAdvMax, back->command.left.phaseAdvMax);
|
||||
if (back) send(MotorController<true, true>::Command::PhaseAdvMax, back->command.right.phaseAdvMax);
|
||||
break;
|
||||
case 8:
|
||||
if (front) send(MotorController<false, false>::Command::CruiseCtrlEna, front->command.left.cruiseCtrlEna);
|
||||
if (front) send(MotorController<false, true>::Command::CruiseCtrlEna, front->command.right.cruiseCtrlEna);
|
||||
if (back) send(MotorController<true, false>::Command::CruiseCtrlEna, back->command.left.cruiseCtrlEna);
|
||||
if (back) send(MotorController<true, true>::Command::CruiseCtrlEna, back->command.right.cruiseCtrlEna);
|
||||
break;
|
||||
case 9:
|
||||
if (front) send(MotorController<false, false>::Command::CruiseMotTgt, front->command.left.nCruiseMotTgt);
|
||||
if (front) send(MotorController<false, true>::Command::CruiseMotTgt, front->command.right.nCruiseMotTgt);
|
||||
if (back) send(MotorController<true, false>::Command::CruiseMotTgt, back->command.left.nCruiseMotTgt);
|
||||
if (back) send(MotorController<true, true>::Command::CruiseMotTgt, back->command.right.nCruiseMotTgt);
|
||||
break;
|
||||
case 10:
|
||||
if (front && send(MotorController<false, false>::Command::BuzzerFreq, front->command.buzzer.freq) == ESP_OK)
|
||||
lastValues.front.freq = front->command.buzzer.freq;
|
||||
// if (front && send(MotorController<false, true>::Command::BuzzerFreq, front->command.buzzer.freq) == ESP_OK)
|
||||
// lastValues.front.freq = front->command.buzzer.freq;
|
||||
if (back && send(MotorController<true, false>::Command::BuzzerFreq, back->command.buzzer.freq) == ESP_OK)
|
||||
lastValues.back.freq = back->command.buzzer.freq;
|
||||
// if (back && send(MotorController<true, true>::Command::BuzzerFreq, back->command.buzzer.freq) == ESP_OK)
|
||||
// lastValues.back.freq = back->command.buzzer.freq;
|
||||
if (front && send(MotorController<false, false>::Command::BuzzerPattern, front->command.buzzer.pattern) == ESP_OK)
|
||||
lastValues.front.pattern = front->command.buzzer.pattern;
|
||||
// if (front && send(MotorController<false, true>::Command::BuzzerPattern, front->command.buzzer.pattern) == ESP_OK)
|
||||
// lastValues.front.pattern = front->command.buzzer.pattern;
|
||||
if (back && send(MotorController<true, false>::Command::BuzzerPattern, back->command.buzzer.pattern) == ESP_OK)
|
||||
lastValues.back.pattern = back->command.buzzer.pattern;
|
||||
// if (back && send(MotorController<true, true>::Command::BuzzerPattern, back->command.buzzer.pattern) == ESP_OK)
|
||||
// lastValues.back.pattern = back->command.buzzer.pattern;
|
||||
break;
|
||||
case 11:
|
||||
if (front) send(MotorController<false, false>::Command::Led, front->command.led);
|
||||
//if (front) send(MotorController<false, true>::Command::Led, front->command.led);
|
||||
if (back) send(MotorController<true, false>::Command::Led, back->command.led);
|
||||
//if (back) send(MotorController<true, true>::Command::Led, back->command.led);
|
||||
if (front) send(MotorController<false, false>::Command::Poweroff, front->command.poweroff);
|
||||
//if (front) send(MotorController<false, true>::Command::Poweroff, front->command.poweroff);
|
||||
if (back) send(MotorController<true, false>::Command::Poweroff, back->command.poweroff);
|
||||
//if (back) send(MotorController<true, true>::Command::Poweroff, back->command.poweroff);
|
||||
break;
|
||||
case 12:
|
||||
if (send(Boardcomputer::Feedback::ButtonLeds, buttonLeds) == ESP_OK)
|
||||
lastValues.buttonLeds = buttonLeds;
|
||||
[[fallthrough]];
|
||||
default:
|
||||
i=0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace can
|
||||
|
||||
Reference in New Issue
Block a user