More registers
This commit is contained in:
254
main.cpp
254
main.cpp
@@ -24,6 +24,7 @@
|
|||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <atomic>
|
#include <atomic>
|
||||||
#include <bit>
|
#include <bit>
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
#include "stm32f1xx_hal.h"
|
#include "stm32f1xx_hal.h"
|
||||||
|
|
||||||
@@ -166,6 +167,11 @@ struct {
|
|||||||
MotorState state;
|
MotorState state;
|
||||||
|
|
||||||
uint32_t chops = 0;
|
uint32_t chops = 0;
|
||||||
|
|
||||||
|
uint8_t hallBits() const
|
||||||
|
{
|
||||||
|
return (rtU.b_hallA ? 0 : 1) | (rtU.b_hallB ? 0 : 2) | (rtU.b_hallC ? 0 : 4);
|
||||||
|
}
|
||||||
} left, right;
|
} left, right;
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
@@ -370,13 +376,12 @@ int main()
|
|||||||
board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;
|
board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;
|
||||||
|
|
||||||
#ifdef FEATURE_SERIAL_FEEDBACK
|
#ifdef FEATURE_SERIAL_FEEDBACK
|
||||||
if (main_loop_counter % 50 == 0) // Send data periodically
|
if (main_loop_counter % 50 == 0)
|
||||||
sendFeedback();
|
sendFeedback();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef FEATURE_CAN
|
#ifdef FEATURE_CAN
|
||||||
if (main_loop_counter % 100 == 0)
|
sendCanFeedback();
|
||||||
sendCanFeedback();
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef FEATURE_BUTTON
|
#ifdef FEATURE_BUTTON
|
||||||
@@ -859,10 +864,10 @@ void CAN_Init()
|
|||||||
sFilterConfig.FilterBank = 0;
|
sFilterConfig.FilterBank = 0;
|
||||||
sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
|
sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
|
||||||
sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
|
sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
|
||||||
sFilterConfig.FilterIdHigh = 0x0000; // TODO
|
sFilterConfig.FilterIdHigh = 0xFFFF; // TODO
|
||||||
sFilterConfig.FilterIdLow = 0x0000; // TODO
|
sFilterConfig.FilterIdLow = 0xFFFF; // TODO
|
||||||
sFilterConfig.FilterMaskIdHigh = 0x0000; // TODO
|
sFilterConfig.FilterMaskIdHigh = 0xFFFF; // TODO
|
||||||
sFilterConfig.FilterMaskIdLow = 0x0000; // TODO
|
sFilterConfig.FilterMaskIdLow = 0xFFFF; // TODO
|
||||||
sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
|
sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
|
||||||
sFilterConfig.FilterActivation = ENABLE;
|
sFilterConfig.FilterActivation = ENABLE;
|
||||||
sFilterConfig.SlaveStartFilterBank = 14;
|
sFilterConfig.SlaveStartFilterBank = 14;
|
||||||
@@ -996,20 +1001,21 @@ void CAN_MspDeInit(CAN_HandleTypeDef *hcan)
|
|||||||
|
|
||||||
void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle)
|
void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle)
|
||||||
{
|
{
|
||||||
myPrintf("CAN_RxFifo0MsgPendingCallback() called");
|
//myPrintf("CAN_RxFifo0MsgPendingCallback() called");
|
||||||
|
|
||||||
CAN_RxHeaderTypeDef header;
|
CAN_RxHeaderTypeDef header;
|
||||||
uint8_t buf[8];
|
uint8_t buf[8];
|
||||||
if (const auto result = HAL_CAN_GetRxMessage(CanHandle, CAN_RX_FIFO0, &header, buf); result == HAL_OK)
|
if (const auto result = HAL_CAN_GetRxMessage(CanHandle, CAN_RX_FIFO0, &header, buf); result == HAL_OK)
|
||||||
{
|
{
|
||||||
myPrintf("HAL_CAN_GetRxMessage() succeeded");
|
myPrintf("StdId=%x %u ExtId=%x %u IDE=%x %u RTR=%x %u DLC=%x %u Timestamp=%x %u FilterMatchIndex=%x %u",
|
||||||
myPrintf("StdId = %x %u", header.StdId, header.StdId);
|
header.StdId, header.StdId,
|
||||||
myPrintf("ExtId = %x %u", header.ExtId, header.ExtId);
|
header.ExtId, header.ExtId,
|
||||||
myPrintf("IDE = %x %u", header.IDE, header.IDE);
|
header.IDE, header.IDE,
|
||||||
myPrintf("RTR = %x %u", header.RTR, header.RTR);
|
header.RTR, header.RTR,
|
||||||
myPrintf("DLC = %x %u", header.DLC, header.DLC);
|
header.DLC, header.DLC,
|
||||||
myPrintf("Timestamp = %x %u", header.Timestamp, header.Timestamp);
|
header.Timestamp, header.Timestamp,
|
||||||
myPrintf("FilterMatchIndex = %x %u", header.FilterMatchIndex, header.FilterMatchIndex);
|
header.FilterMatchIndex, header.FilterMatchIndex
|
||||||
|
);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
@@ -1524,99 +1530,163 @@ void sendFeedback()
|
|||||||
void sendCanFeedback()
|
void sendCanFeedback()
|
||||||
{
|
{
|
||||||
myPrintf("sendCanFeedback() called");
|
myPrintf("sendCanFeedback() called");
|
||||||
while (const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle))
|
|
||||||
{
|
|
||||||
myPrintf("free=%u", free);
|
|
||||||
|
|
||||||
if (free < 1)
|
const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle);
|
||||||
return;
|
if (!free)
|
||||||
|
return;
|
||||||
|
|
||||||
enum SendState : uint8_t {
|
enum { // vv
|
||||||
LeftCurrent,
|
DeviceTypeMotorController = 0b00000000000
|
||||||
RightCurrent,
|
};
|
||||||
LeftSpeed,
|
|
||||||
RightSpeed,
|
|
||||||
Restart
|
|
||||||
};
|
|
||||||
|
|
||||||
static union {
|
enum { // ..vv
|
||||||
SendState sendState{LeftCurrent};
|
MotorControllerRec = 0b00000000000,
|
||||||
uint8_t asUint8;
|
MotorControllerSend = 0b00010000000,
|
||||||
};
|
};
|
||||||
|
|
||||||
constexpr auto send = [](uint32_t addr, auto value){
|
enum { // ....vvvvv
|
||||||
CAN_TxHeaderTypeDef header;
|
MotorControllerDcLink = 0b00000000000,
|
||||||
header.StdId = addr;
|
MotorControllerSpeed = 0b00000000100,
|
||||||
header.ExtId = 0x01;
|
MotorControllerError = 0b00000001000,
|
||||||
header.RTR = CAN_RTR_DATA;
|
MotorControllerAngle = 0b00000001100,
|
||||||
header.IDE = CAN_ID_STD;
|
MotorControllerDcPhaA = 0b00000010000,
|
||||||
header.DLC = 8;
|
MotorControllerDcPhaB = 0b00000010100,
|
||||||
header.TransmitGlobalTime = DISABLE;
|
MotorControllerDcPhaC = 0b00000011000,
|
||||||
|
MotorControllerChops = 0b00000011100,
|
||||||
|
MotorControllerHall = 0b00000100000
|
||||||
|
};
|
||||||
|
|
||||||
uint8_t buf[8];
|
enum { // .........v
|
||||||
std::fill(std::begin(buf), std::end(buf), 0x69);
|
MotorControllerFront = 0b00000000000,
|
||||||
|
MotorControllerBack = 0b00000000010,
|
||||||
|
};
|
||||||
|
|
||||||
static uint32_t TxMailbox;
|
enum { // ..........v
|
||||||
if (const auto result = HAL_CAN_AddTxMessage(&CanHandle, &header, buf, &TxMailbox); result != HAL_OK)
|
MotorControllerLeft = 0b00000000000,
|
||||||
{
|
MotorControllerRight = 0b00000000001,
|
||||||
myPrintf("HAL_CAN_AddTxMessage() failed with %i", result);
|
};
|
||||||
while (true);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
enum { // vv
|
enum {
|
||||||
DeviceTypeMotorController = 0b00000000000
|
MotorControllerFrontLeftDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerFront | MotorControllerLeft,
|
||||||
};
|
MotorControllerFrontRightDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerFront | MotorControllerRight,
|
||||||
|
MotorControllerBackLeftDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerBack | MotorControllerLeft,
|
||||||
|
MotorControllerBackRightDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerBack | MotorControllerRight,
|
||||||
|
|
||||||
enum { // ..vv
|
MotorControllerFrontLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerFront | MotorControllerLeft,
|
||||||
MotorControllerRec = 0b00000000000,
|
MotorControllerFrontRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerFront | MotorControllerRight,
|
||||||
MotorControllerSend = 0b00010000000,
|
MotorControllerBackLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerBack | MotorControllerLeft,
|
||||||
};
|
MotorControllerBackRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerBack | MotorControllerRight,
|
||||||
|
|
||||||
enum { // ....v
|
MotorControllerFrontLeftError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerFront | MotorControllerLeft,
|
||||||
MotorControllerFront = 0b00000000000,
|
MotorControllerFrontRightError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerFront | MotorControllerRight,
|
||||||
MotorControllerBack = 0b00001000000,
|
MotorControllerBackLeftError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerBack | MotorControllerLeft,
|
||||||
};
|
MotorControllerBackRightError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerBack | MotorControllerRight,
|
||||||
|
|
||||||
enum { // .....v
|
MotorControllerFrontLeftAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerFront | MotorControllerLeft,
|
||||||
MotorControllerLeft = 0b00000000000,
|
MotorControllerFrontRightAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerFront | MotorControllerRight,
|
||||||
MotorControllerRight = 0b00000100000,
|
MotorControllerBackLeftAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerBack | MotorControllerLeft,
|
||||||
};
|
MotorControllerBackRightAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerBack | MotorControllerRight,
|
||||||
|
|
||||||
enum { // ......vvvvv
|
MotorControllerFrontLeftDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerFront | MotorControllerLeft,
|
||||||
MotorControllerCurrent = 0b00000000000,
|
MotorControllerFrontRightDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerFront | MotorControllerRight,
|
||||||
MotorControllerSpeed = 0b00000000001
|
MotorControllerBackLeftDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerBack | MotorControllerLeft,
|
||||||
};
|
MotorControllerBackRightDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerBack | MotorControllerRight,
|
||||||
|
|
||||||
enum {
|
MotorControllerFrontLeftDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerFront | MotorControllerLeft,
|
||||||
MotorControllerFrontLeftCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerLeft | MotorControllerFront,
|
MotorControllerFrontRightDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerFront | MotorControllerRight,
|
||||||
MotorControllerFrontRightCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerRight | MotorControllerFront,
|
MotorControllerBackLeftDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerBack | MotorControllerLeft,
|
||||||
MotorControllerBackLeftCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerLeft | MotorControllerBack,
|
MotorControllerBackRightDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerBack | MotorControllerRight,
|
||||||
MotorControllerBackRightCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerRight | MotorControllerBack,
|
|
||||||
MotorControllerFrontLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerLeft | MotorControllerFront,
|
|
||||||
MotorControllerFrontRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerRight | MotorControllerFront,
|
|
||||||
MotorControllerBackLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerLeft | MotorControllerBack,
|
|
||||||
MotorControllerBackRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerRight | MotorControllerBack,
|
|
||||||
};
|
|
||||||
|
|
||||||
switch (sendState)
|
MotorControllerFrontLeftDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerFront | MotorControllerLeft,
|
||||||
|
MotorControllerFrontRightDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerFront | MotorControllerRight,
|
||||||
|
MotorControllerBackLeftDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerBack | MotorControllerLeft,
|
||||||
|
MotorControllerBackRightDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerBack | MotorControllerRight,
|
||||||
|
|
||||||
|
MotorControllerFrontLeftChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerFront | MotorControllerLeft,
|
||||||
|
MotorControllerFrontRightChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerFront | MotorControllerRight,
|
||||||
|
MotorControllerBackLeftChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerBack | MotorControllerLeft,
|
||||||
|
MotorControllerBackRightChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerBack | MotorControllerRight,
|
||||||
|
|
||||||
|
MotorControllerFrontLeftHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerFront | MotorControllerLeft,
|
||||||
|
MotorControllerFrontRightHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerFront | MotorControllerRight,
|
||||||
|
MotorControllerBackLeftHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerBack | MotorControllerLeft,
|
||||||
|
MotorControllerBackRightHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerBack | MotorControllerRight,
|
||||||
|
};
|
||||||
|
|
||||||
|
enum SendState : uint8_t {
|
||||||
|
LeftDcLink,
|
||||||
|
RightDcLink,
|
||||||
|
LeftSpeed,
|
||||||
|
RightSpeed,
|
||||||
|
LeftError,
|
||||||
|
RightError,
|
||||||
|
LeftAngle,
|
||||||
|
RightAngle,
|
||||||
|
LeftDcPhaA,
|
||||||
|
RightDcPhaA,
|
||||||
|
LeftDcPhaB,
|
||||||
|
RightDcPhaB,
|
||||||
|
LeftDcPhaC,
|
||||||
|
RightDcPhaC,
|
||||||
|
LeftChops,
|
||||||
|
RightChops,
|
||||||
|
LeftHall,
|
||||||
|
RightHall,
|
||||||
|
Restart
|
||||||
|
};
|
||||||
|
|
||||||
|
static union {
|
||||||
|
SendState sendState{LeftDcLink};
|
||||||
|
uint8_t asUint8;
|
||||||
|
};
|
||||||
|
|
||||||
|
constexpr auto send = [](uint32_t addr, auto value){
|
||||||
|
CAN_TxHeaderTypeDef header;
|
||||||
|
header.StdId = addr;
|
||||||
|
header.ExtId = 0x01;
|
||||||
|
header.RTR = CAN_RTR_DATA;
|
||||||
|
header.IDE = CAN_ID_STD;
|
||||||
|
header.DLC = sizeof(value);
|
||||||
|
header.TransmitGlobalTime = DISABLE;
|
||||||
|
|
||||||
|
uint8_t buf[8] {0};
|
||||||
|
std::memcpy(buf, &value, sizeof(value));
|
||||||
|
|
||||||
|
static uint32_t TxMailbox;
|
||||||
|
if (const auto result = HAL_CAN_AddTxMessage(&CanHandle, &header, buf, &TxMailbox); result != HAL_OK)
|
||||||
{
|
{
|
||||||
#ifdef VORNE
|
myPrintf("HAL_CAN_AddTxMessage() failed with %i", result);
|
||||||
case LeftCurrent: send(0b00010000000, left.rtU.i_DCLink); break;
|
//while (true);
|
||||||
case RightCurrent: send(0b00010100000, right.rtU.i_DCLink); break;
|
|
||||||
#else
|
|
||||||
case LeftCurrent: send(0b00011000000, left.rtU.i_DCLink); break;
|
|
||||||
case RightCurrent: send(0b00011100000, right.rtU.i_DCLink); break;
|
|
||||||
#endif
|
|
||||||
default:
|
|
||||||
__builtin_unreachable();
|
|
||||||
}
|
}
|
||||||
|
};
|
||||||
|
|
||||||
asUint8++;
|
switch (sendState)
|
||||||
|
{
|
||||||
if (sendState >= Restart)
|
case LeftDcLink: myPrintf("LeftCurrent free=%u", free); send(MotorControllerFrontLeftDcLink, left. rtU.i_DCLink); break;
|
||||||
sendState = LeftCurrent;
|
case RightDcLink: myPrintf("RightCurrent free=%u", free); send(MotorControllerFrontRightDcLink, right.rtU.i_DCLink); break;
|
||||||
|
case LeftSpeed: myPrintf("LeftSpeed free=%u", free); send(MotorControllerFrontLeftSpeed, left. rtY.n_mot); break;
|
||||||
|
case RightSpeed: myPrintf("RightSpeed free=%u", free); send(MotorControllerFrontRightSpeed, right.rtY.n_mot); break;
|
||||||
|
case LeftError: myPrintf("LeftError free=%u", free); send(MotorControllerFrontLeftError, left. rtY.z_errCode); break;
|
||||||
|
case RightError: myPrintf("RightError free=%u", free); send(MotorControllerFrontRightError, right.rtY.z_errCode); break;
|
||||||
|
case LeftAngle: myPrintf("LeftAngle free=%u", free); send(MotorControllerFrontLeftAngle, left. rtY.a_elecAngle); break;
|
||||||
|
case RightAngle: myPrintf("RightAngle free=%u", free); send(MotorControllerFrontRightAngle, right.rtY.a_elecAngle); break;
|
||||||
|
case LeftDcPhaA: myPrintf("LeftDcPhaA free=%u", free); send(MotorControllerFrontLeftDcPhaA, left. rtY.DC_phaA); break;
|
||||||
|
case RightDcPhaA: myPrintf("RightDcPhaA free=%u", free); send(MotorControllerFrontRightDcPhaA, right.rtY.DC_phaA); break;
|
||||||
|
case LeftDcPhaB: myPrintf("LeftDcPhaB free=%u", free); send(MotorControllerFrontLeftDcPhaB, left. rtY.DC_phaB); break;
|
||||||
|
case RightDcPhaB: myPrintf("RightDcPhaB free=%u", free); send(MotorControllerFrontRightDcPhaB, right.rtY.DC_phaB); break;
|
||||||
|
case LeftDcPhaC: myPrintf("LeftDcPhaC free=%u", free); send(MotorControllerFrontLeftDcPhaC, left. rtY.DC_phaC); break;
|
||||||
|
case RightDcPhaC: myPrintf("RightDcPhaC free=%u", free); send(MotorControllerFrontRightDcPhaC, right.rtY.DC_phaC); break;
|
||||||
|
case LeftChops: myPrintf("LeftChops free=%u", free); send(MotorControllerFrontLeftChops, left. chops); break;
|
||||||
|
case RightChops: myPrintf("RightChops free=%u", free); send(MotorControllerFrontRightChops, right.chops); break;
|
||||||
|
case LeftHall: myPrintf("LeftHall free=%u", free); send(MotorControllerFrontLeftHall, left.hallBits()); break;
|
||||||
|
case RightHall: myPrintf("RightHall free=%u", free); send(MotorControllerFrontRightHall, right.hallBits()); break;
|
||||||
|
default: __builtin_unreachable();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
asUint8++;
|
||||||
|
|
||||||
|
if (sendState >= Restart)
|
||||||
|
sendState = LeftDcLink;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user