mirror of
https://github.com/supleed2/ELEC60013-ES-CW2.git
synced 2024-12-22 13:45:51 +00:00
Merge to main
This commit is contained in:
parent
e952f4ccb6
commit
5a6c49ca89
31
lib/es_can/es_can
Normal file
31
lib/es_can/es_can
Normal file
|
@ -0,0 +1,31 @@
|
||||||
|
#include <cstdint>
|
||||||
|
|
||||||
|
#ifndef ES_CAN_H
|
||||||
|
#define ES_CAN_H
|
||||||
|
|
||||||
|
// Initialise the CAN module
|
||||||
|
uint32_t CAN_Init(bool loopback = false);
|
||||||
|
|
||||||
|
// Enable the CAN module
|
||||||
|
uint32_t CAN_Start();
|
||||||
|
|
||||||
|
// Set up a recevie filter
|
||||||
|
// Defaults to receive everything
|
||||||
|
uint32_t setCANFilter(uint32_t filterID = 0, uint32_t maskID = 0, uint32_t filterBank = 0);
|
||||||
|
|
||||||
|
// Send a message
|
||||||
|
uint32_t CAN_TX(uint32_t ID, uint8_t data[8]);
|
||||||
|
|
||||||
|
// Get the number of received messages
|
||||||
|
uint32_t CAN_CheckRXLevel();
|
||||||
|
|
||||||
|
// Get a received message from the FIFO
|
||||||
|
uint32_t CAN_RX(uint32_t &ID, uint8_t data[8]);
|
||||||
|
|
||||||
|
// Set up an interrupt on received messages
|
||||||
|
uint32_t CAN_RegisterRX_ISR(void (&callback)());
|
||||||
|
|
||||||
|
// Set up an interrupt on transmitted messages
|
||||||
|
uint32_t CAN_RegisterTX_ISR(void (&callback)());
|
||||||
|
|
||||||
|
#endif
|
202
lib/es_can/es_can.cpp
Normal file
202
lib/es_can/es_can.cpp
Normal file
|
@ -0,0 +1,202 @@
|
||||||
|
#include <es_can>
|
||||||
|
#include <stm32l4xx_hal_can.h>
|
||||||
|
#include <stm32l4xx_hal_cortex.h>
|
||||||
|
#include <stm32l4xx_hal_gpio.h>
|
||||||
|
#include <stm32l4xx_hal_rcc.h>
|
||||||
|
|
||||||
|
// Overwrite the weak default IRQ Handlers and callabcks
|
||||||
|
extern "C" void CAN1_RX0_IRQHandler(void);
|
||||||
|
extern "C" void CAN1_TX_IRQHandler(void);
|
||||||
|
|
||||||
|
// Pointer to user ISRS
|
||||||
|
void (*CAN_RX_ISR)() = NULL;
|
||||||
|
void (*CAN_TX_ISR)() = NULL;
|
||||||
|
|
||||||
|
// CAN handle struct with initialisation parameters
|
||||||
|
// Timing from http://www.bittiming.can-wiki.info/ with bit rate = 125kHz and clock frequency = 80MHz
|
||||||
|
CAN_HandleTypeDef CAN_Handle = {
|
||||||
|
CAN1,
|
||||||
|
{
|
||||||
|
40, // Prescaler
|
||||||
|
CAN_MODE_NORMAL, // Normal/loopback/silent mode
|
||||||
|
CAN_SJW_2TQ, // SyncJumpWidth
|
||||||
|
CAN_BS1_13TQ, // TimeSeg1
|
||||||
|
CAN_BS2_2TQ, // TimeSeg2
|
||||||
|
DISABLE, // TimeTriggeredMode
|
||||||
|
DISABLE, // AutoBusOff
|
||||||
|
ENABLE, // AutoWakeUp
|
||||||
|
ENABLE, // AutoRetransmission
|
||||||
|
DISABLE, // ReceiveFifoLocked
|
||||||
|
ENABLE // TransmitFifoPriority
|
||||||
|
},
|
||||||
|
HAL_CAN_STATE_RESET, // State
|
||||||
|
HAL_CAN_ERROR_NONE // Error Code
|
||||||
|
};
|
||||||
|
|
||||||
|
// Initialise CAN dependencies: GPIO and clock
|
||||||
|
void HAL_CAN_MspInit(CAN_HandleTypeDef *CAN_Handle) {
|
||||||
|
|
||||||
|
// Set up the pin initialisation
|
||||||
|
GPIO_InitTypeDef GPIO_InitCAN_TX = {
|
||||||
|
GPIO_PIN_12, // PA12 is CAN TX
|
||||||
|
GPIO_MODE_AF_PP, // Alternate function, push-pull driver
|
||||||
|
GPIO_NOPULL, // No pull-up
|
||||||
|
GPIO_SPEED_FREQ_MEDIUM, // Medium slew rate
|
||||||
|
GPIO_AF9_CAN1 // Alternate function is CAN
|
||||||
|
};
|
||||||
|
|
||||||
|
GPIO_InitTypeDef GPIO_InitCAN_RX = {
|
||||||
|
GPIO_PIN_11, // PA11 is CAN RX
|
||||||
|
GPIO_MODE_AF_PP, // Alternate function, push-pull driver
|
||||||
|
GPIO_PULLUP, // Pull-up enabled
|
||||||
|
GPIO_SPEED_FREQ_MEDIUM, // Medium slew rate
|
||||||
|
GPIO_AF9_CAN1 // Alternate function is CAN
|
||||||
|
};
|
||||||
|
|
||||||
|
// Enable the CAN and GPIO clocks
|
||||||
|
__HAL_RCC_CAN1_CLK_ENABLE(); // Enable the CAN interface clock
|
||||||
|
__HAL_RCC_GPIOA_CLK_ENABLE(); // Enable the clock for the CAN GPIOs
|
||||||
|
|
||||||
|
// Initialise the pins
|
||||||
|
HAL_GPIO_Init(GPIOA, &GPIO_InitCAN_TX); // Configure CAN pin
|
||||||
|
HAL_GPIO_Init(GPIOA, &GPIO_InitCAN_RX); // Configure CAN pin
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t CAN_Init(bool loopback) {
|
||||||
|
if (loopback)
|
||||||
|
CAN_Handle.Init.Mode = CAN_MODE_LOOPBACK;
|
||||||
|
return (uint32_t)HAL_CAN_Init(&CAN_Handle);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t setCANFilter(uint32_t filterID, uint32_t maskID, uint32_t filterBank) {
|
||||||
|
|
||||||
|
// Set up the filter definition
|
||||||
|
CAN_FilterTypeDef filterInfo = {
|
||||||
|
(filterID << 5) & 0xffe0, // Filter ID
|
||||||
|
0, // Filter ID LSBs = 0
|
||||||
|
(maskID << 5) & 0xffe0, // Mask MSBs
|
||||||
|
0, // Mask LSBs = 0
|
||||||
|
0, // FIFO selection
|
||||||
|
filterBank & 0xf, // Filter bank selection
|
||||||
|
CAN_FILTERMODE_IDMASK, // Mask mode
|
||||||
|
CAN_FILTERSCALE_32BIT, // 32 bit IDs
|
||||||
|
CAN_FILTER_ENABLE, // Enable filter
|
||||||
|
0 // uint32_t SlaveStartFilterBank
|
||||||
|
};
|
||||||
|
|
||||||
|
return (uint32_t)HAL_CAN_ConfigFilter(&CAN_Handle, &filterInfo);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t CAN_Start() {
|
||||||
|
return (uint32_t)HAL_CAN_Start(&CAN_Handle);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t CAN_TX(uint32_t ID, uint8_t data[8]) {
|
||||||
|
|
||||||
|
// Set up the message header
|
||||||
|
CAN_TxHeaderTypeDef txHeader = {
|
||||||
|
ID & 0x7ff, // Standard ID
|
||||||
|
0, // Ext ID = 0
|
||||||
|
CAN_ID_STD, // Use Standard ID
|
||||||
|
CAN_RTR_DATA, // Data Frame
|
||||||
|
8, // Send 8 bytes
|
||||||
|
DISABLE // No time triggered mode
|
||||||
|
};
|
||||||
|
|
||||||
|
// Wait for free mailbox
|
||||||
|
while (!HAL_CAN_GetTxMailboxesFreeLevel(&CAN_Handle))
|
||||||
|
;
|
||||||
|
|
||||||
|
// Start the transmission
|
||||||
|
return (uint32_t)HAL_CAN_AddTxMessage(&CAN_Handle, &txHeader, data, NULL);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t CAN_CheckRXLevel() {
|
||||||
|
return HAL_CAN_GetRxFifoFillLevel(&CAN_Handle, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t CAN_RX(uint32_t &ID, uint8_t data[8]) {
|
||||||
|
CAN_RxHeaderTypeDef rxHeader;
|
||||||
|
|
||||||
|
// Wait for message in FIFO
|
||||||
|
while (!HAL_CAN_GetRxFifoFillLevel(&CAN_Handle, 0))
|
||||||
|
;
|
||||||
|
|
||||||
|
// Get the message from the FIFO
|
||||||
|
uint32_t result = (uint32_t)HAL_CAN_GetRxMessage(&CAN_Handle, 0, &rxHeader, data);
|
||||||
|
|
||||||
|
// Store the ID from the header
|
||||||
|
ID = rxHeader.StdId;
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t CAN_RegisterRX_ISR(void (&callback)()) {
|
||||||
|
// Store pointer to user ISR
|
||||||
|
CAN_RX_ISR = &callback;
|
||||||
|
|
||||||
|
// Enable message received interrupt in HAL
|
||||||
|
uint32_t status = (uint32_t)HAL_CAN_ActivateNotification(&CAN_Handle, CAN_IT_RX_FIFO0_MSG_PENDING);
|
||||||
|
|
||||||
|
// Switch on the interrupt
|
||||||
|
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 6, 0);
|
||||||
|
HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
|
||||||
|
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t CAN_RegisterTX_ISR(void (&callback)()) {
|
||||||
|
// Store pointer to user ISR
|
||||||
|
CAN_TX_ISR = &callback;
|
||||||
|
|
||||||
|
// Enable message received interrupt in HAL
|
||||||
|
uint32_t status = (uint32_t)HAL_CAN_ActivateNotification(&CAN_Handle, CAN_IT_TX_MAILBOX_EMPTY);
|
||||||
|
|
||||||
|
// Switch on the interrupt
|
||||||
|
HAL_NVIC_SetPriority(CAN1_TX_IRQn, 6, 0);
|
||||||
|
HAL_NVIC_EnableIRQ(CAN1_TX_IRQn);
|
||||||
|
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
|
||||||
|
|
||||||
|
// Call the user ISR if it has been registered
|
||||||
|
if (CAN_RX_ISR)
|
||||||
|
CAN_RX_ISR();
|
||||||
|
}
|
||||||
|
|
||||||
|
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {
|
||||||
|
|
||||||
|
// Call the user ISR if it has been registered
|
||||||
|
if (CAN_TX_ISR)
|
||||||
|
CAN_TX_ISR();
|
||||||
|
}
|
||||||
|
|
||||||
|
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) {
|
||||||
|
|
||||||
|
// Call the user ISR if it has been registered
|
||||||
|
if (CAN_TX_ISR)
|
||||||
|
CAN_TX_ISR();
|
||||||
|
}
|
||||||
|
|
||||||
|
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) {
|
||||||
|
|
||||||
|
// Call the user ISR if it has been registered
|
||||||
|
if (CAN_TX_ISR)
|
||||||
|
CAN_TX_ISR();
|
||||||
|
}
|
||||||
|
|
||||||
|
// This is the base ISR at the interrupt vector
|
||||||
|
void CAN1_RX0_IRQHandler(void) {
|
||||||
|
|
||||||
|
// Use the HAL interrupt handler
|
||||||
|
HAL_CAN_IRQHandler(&CAN_Handle);
|
||||||
|
}
|
||||||
|
|
||||||
|
// This is the base ISR at the interrupt vector
|
||||||
|
void CAN1_TX_IRQHandler(void) {
|
||||||
|
|
||||||
|
// Use the HAL interrupt handler
|
||||||
|
HAL_CAN_IRQHandler(&CAN_Handle);
|
||||||
|
}
|
72
src/main.cpp
72
src/main.cpp
|
@ -2,6 +2,7 @@
|
||||||
#include <STM32FreeRTOS.h>
|
#include <STM32FreeRTOS.h>
|
||||||
#include <U8g2lib.h>
|
#include <U8g2lib.h>
|
||||||
#include <atomic>
|
#include <atomic>
|
||||||
|
#include <es_can>
|
||||||
#include <knob>
|
#include <knob>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
|
@ -10,9 +11,12 @@
|
||||||
const uint32_t interval = 10; // Display update interval
|
const uint32_t interval = 10; // Display update interval
|
||||||
const uint8_t octave = 4; // Octave to start on
|
const uint8_t octave = 4; // Octave to start on
|
||||||
const uint32_t samplingRate = 44100; // Sampling rate
|
const uint32_t samplingRate = 44100; // Sampling rate
|
||||||
|
const uint32_t canID = 0x123;
|
||||||
// Variables
|
// Variables
|
||||||
std::atomic<int32_t> currentStepSize;
|
std::atomic<int32_t> currentStepSize;
|
||||||
std::atomic<uint8_t> keyArray[7];
|
std::atomic<uint8_t> keyArray[7];
|
||||||
|
QueueHandle_t msgInQ;
|
||||||
|
uint8_t RX_Message[8] = {0};
|
||||||
// Objects
|
// Objects
|
||||||
U8G2_SSD1305_128X32_NONAME_F_HW_I2C u8g2(U8G2_R0); // Display driver object
|
U8G2_SSD1305_128X32_NONAME_F_HW_I2C u8g2(U8G2_R0); // Display driver object
|
||||||
Knob K3 = Knob(0, 16); // Knob driver object
|
Knob K3 = Knob(0, 16); // Knob driver object
|
||||||
|
@ -120,7 +124,7 @@ uint16_t getTopKey() {
|
||||||
for (uint8_t i = 0; i < 3; i++) {
|
for (uint8_t i = 0; i < 3; i++) {
|
||||||
for (uint8_t j = 0; j < 4; j++) {
|
for (uint8_t j = 0; j < 4; j++) {
|
||||||
if (keyArray[i] & (0x1 << j)) {
|
if (keyArray[i] & (0x1 << j)) {
|
||||||
topKey = (octave - 2) * 12 + i * 4 + j + 1;
|
topKey = (octave - 1) * 12 + i * 4 + j + 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -135,6 +139,36 @@ void sampleISR() {
|
||||||
analogWrite(OUTR_PIN, Vout + 128);
|
analogWrite(OUTR_PIN, Vout + 128);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void CAN_RX_ISR() {
|
||||||
|
uint8_t ISR_RX_Message[8];
|
||||||
|
uint32_t ISR_rxID;
|
||||||
|
CAN_RX(ISR_rxID, ISR_RX_Message);
|
||||||
|
xQueueSendFromISR(msgInQ, ISR_RX_Message, nullptr);
|
||||||
|
}
|
||||||
|
|
||||||
|
void decodeTask(void *pvParameters) {
|
||||||
|
while (1) {
|
||||||
|
xQueueReceive(msgInQ, RX_Message, portMAX_DELAY);
|
||||||
|
if (RX_Message[0] == 0x50) { // Pressed
|
||||||
|
currentStepSize = notes[(RX_Message[1] - 1) * 12 + RX_Message[2]].stepSize;
|
||||||
|
} else { // Released
|
||||||
|
currentStepSize = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void keyChangedSendTXMessage(uint8_t octave, uint8_t key, bool pressed) {
|
||||||
|
uint8_t TX_Message[8] = {0};
|
||||||
|
if (pressed) {
|
||||||
|
TX_Message[0] = 0x50; // "P"
|
||||||
|
} else {
|
||||||
|
TX_Message[0] = 0x52; // "R"
|
||||||
|
}
|
||||||
|
TX_Message[1] = octave;
|
||||||
|
TX_Message[2] = key;
|
||||||
|
CAN_TX(canID, TX_Message);
|
||||||
|
}
|
||||||
|
|
||||||
// Task to update keyArray values at a higher priority
|
// Task to update keyArray values at a higher priority
|
||||||
void scanKeysTask(void *pvParameters) {
|
void scanKeysTask(void *pvParameters) {
|
||||||
const TickType_t xFrequency = 50 / portTICK_PERIOD_MS;
|
const TickType_t xFrequency = 50 / portTICK_PERIOD_MS;
|
||||||
|
@ -143,8 +177,19 @@ void scanKeysTask(void *pvParameters) {
|
||||||
vTaskDelayUntil(&xLastWakeTime, xFrequency);
|
vTaskDelayUntil(&xLastWakeTime, xFrequency);
|
||||||
for (uint8_t i = 0; i < 7; i++) {
|
for (uint8_t i = 0; i < 7; i++) {
|
||||||
setRow(i);
|
setRow(i);
|
||||||
|
uint8_t oldRow = keyArray[i];
|
||||||
delayMicroseconds(3);
|
delayMicroseconds(3);
|
||||||
keyArray[i] = readCols();
|
uint8_t newRow = readCols();
|
||||||
|
if (oldRow == newRow) {
|
||||||
|
continue;
|
||||||
|
} else {
|
||||||
|
keyArray[i] = newRow;
|
||||||
|
for (uint8_t j = 0; j < 4; j++) {
|
||||||
|
if ((oldRow & (0x1 << j)) ^ (newRow & (0x1 << j))) {
|
||||||
|
keyChangedSendTXMessage(octave, i * 4 + j + 1, newRow & (0x1 << j));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
currentStepSize = notes[getTopKey()].stepSize; // Atomic Store
|
currentStepSize = notes[getTopKey()].stepSize; // Atomic Store
|
||||||
K3.updateRotation(keyArray[3] & 0x1, keyArray[3] & 0x2);
|
K3.updateRotation(keyArray[3] & 0x1, keyArray[3] & 0x2);
|
||||||
|
@ -157,6 +202,8 @@ void displayUpdateTask(void *pvParameters) {
|
||||||
TickType_t xLastWakeTime = xTaskGetTickCount();
|
TickType_t xLastWakeTime = xTaskGetTickCount();
|
||||||
while (1) {
|
while (1) {
|
||||||
vTaskDelayUntil(&xLastWakeTime, xFrequency);
|
vTaskDelayUntil(&xLastWakeTime, xFrequency);
|
||||||
|
uint32_t rxID;
|
||||||
|
|
||||||
u8g2.clearBuffer(); // clear the internal memory
|
u8g2.clearBuffer(); // clear the internal memory
|
||||||
u8g2.setFont(u8g2_font_profont12_mf); // choose a suitable font
|
u8g2.setFont(u8g2_font_profont12_mf); // choose a suitable font
|
||||||
uint16_t key = getTopKey();
|
uint16_t key = getTopKey();
|
||||||
|
@ -166,7 +213,11 @@ void displayUpdateTask(void *pvParameters) {
|
||||||
for (uint8_t i = 0; i < 7; i++) {
|
for (uint8_t i = 0; i < 7; i++) {
|
||||||
u8g2.print(keyArray[i], HEX);
|
u8g2.print(keyArray[i], HEX);
|
||||||
}
|
}
|
||||||
u8g2.drawXBM(118, 0, 10, 10, icon_bits);
|
// u8g2.drawXBM(118, 0, 10, 10, icon_bits);
|
||||||
|
u8g2.setCursor(100, 10);
|
||||||
|
u8g2.print((char)RX_Message[0]);
|
||||||
|
u8g2.print(RX_Message[1]);
|
||||||
|
u8g2.print(RX_Message[2], HEX);
|
||||||
u8g2.setCursor(2, 30);
|
u8g2.setCursor(2, 30);
|
||||||
u8g2.print(K3.getRotation());
|
u8g2.print(K3.getRotation());
|
||||||
u8g2.sendBuffer(); // transfer internal memory to the display
|
u8g2.sendBuffer(); // transfer internal memory to the display
|
||||||
|
@ -201,19 +252,26 @@ void setup() {
|
||||||
Serial.begin(115200);
|
Serial.begin(115200);
|
||||||
Serial.println("Hello World");
|
Serial.println("Hello World");
|
||||||
#pragma endregion
|
#pragma endregion
|
||||||
|
#pragma region CAN Setup
|
||||||
|
msgInQ = xQueueCreate(36, 8);
|
||||||
|
CAN_Init(true);
|
||||||
|
setCANFilter(0x123, 0x7ff);
|
||||||
|
CAN_RegisterRX_ISR(CAN_RX_ISR);
|
||||||
|
CAN_Start();
|
||||||
|
#pragma endregion
|
||||||
#pragma region Task Scheduler Setup
|
#pragma region Task Scheduler Setup
|
||||||
TIM_TypeDef *Instance = TIM1;
|
TIM_TypeDef *Instance = TIM1;
|
||||||
HardwareTimer *sampleTimer = new HardwareTimer(Instance);
|
HardwareTimer *sampleTimer = new HardwareTimer(Instance);
|
||||||
sampleTimer->setOverflow(samplingRate, HERTZ_FORMAT);
|
sampleTimer->setOverflow(samplingRate, HERTZ_FORMAT);
|
||||||
sampleTimer->attachInterrupt(sampleISR);
|
sampleTimer->attachInterrupt(sampleISR);
|
||||||
sampleTimer->resume();
|
sampleTimer->resume();
|
||||||
TaskHandle_t scanKeysHandle = NULL;
|
TaskHandle_t scanKeysHandle = nullptr;
|
||||||
TaskHandle_t displayUpdateHandle = NULL;
|
TaskHandle_t displayUpdateHandle = nullptr;
|
||||||
xTaskCreate(
|
xTaskCreate(
|
||||||
scanKeysTask, // Function that implements the task
|
scanKeysTask, // Function that implements the task
|
||||||
"scanKeys", // Text name for the task
|
"scanKeys", // Text name for the task
|
||||||
64, // Stack size in words, not bytes
|
64, // Stack size in words, not bytes
|
||||||
NULL, // Parameter passed into the task
|
nullptr, // Parameter passed into the task
|
||||||
2, // Task priority
|
2, // Task priority
|
||||||
&scanKeysHandle // Pointer to store the task handle
|
&scanKeysHandle // Pointer to store the task handle
|
||||||
);
|
);
|
||||||
|
@ -221,7 +279,7 @@ void setup() {
|
||||||
displayUpdateTask, // Function that implements the task
|
displayUpdateTask, // Function that implements the task
|
||||||
"displayUpdate", // Text name for the task
|
"displayUpdate", // Text name for the task
|
||||||
256, // Stack size in words, not bytes
|
256, // Stack size in words, not bytes
|
||||||
NULL, // Parameter passed into the task
|
nullptr, // Parameter passed into the task
|
||||||
1, // Task priority
|
1, // Task priority
|
||||||
&displayUpdateHandle // Pointer to store the task handle
|
&displayUpdateHandle // Pointer to store the task handle
|
||||||
);
|
);
|
||||||
|
|
Loading…
Reference in a new issue