- Updated CMakeLists.txt and board config .h files
- Refactored core0.c to most functions are now static functions. - Introduced new structures (`dcc_packet_t` and `dcc_ring_buffer_t`) for DCC packet data handling and packet detection. - DCC pin interrupt now only does packet detection and no evaluation, evaluation is now done inside the main loop. - Refactor message evaluation logic. - Implemented hard fault handler, now rebooting when hardfault is encountered. - Optimized initialization sequence in main(). - Controller and speed helper functions now run outside the timer interrupts, instead flags are set, which are checked inside the core1 main loop. - Added enumeration for speed steps to improve readability. (However they are yet to be used) - Added various documentation comments. - Added various LOG() messages. - Added python script for generating speed_step typedef definition.
This commit is contained in:
1 parent
acdbf3cffe
commit
d970170795
10 files changed
+847
-300
No files matched your search
+13
-7
@@ -23,8 +23,12 @@ if (EXISTS ${picoVscode})
|
||||
include(${picoVscode})
|
||||
endif()
|
||||
# ====================================================================================
|
||||
|
||||
set(PICO_BOARD_HEADER_DIRS ${CMAKE_SOURCE_DIR})
|
||||
set(PICO_BOARD RP2040-Decoder-board CACHE STRING "Board type")
|
||||
# Board type selection:
|
||||
#
|
||||
# Please select RP2040-Decoder-board.h for Rev >= 1.0 or RP2040-Decoder-board-legacy.h for Rev < 1.0
|
||||
set(PICO_BOARD RP2040-Decoder-board.h CACHE STRING "Board type")
|
||||
|
||||
# Pull in Raspberry Pi Pico SDK (must be before project)
|
||||
include(pico_sdk_import.cmake)
|
||||
@@ -34,8 +38,9 @@ project(RP2040-Decoder C CXX ASM)
|
||||
# Initialise the Raspberry Pi Pico SDK
|
||||
pico_sdk_init()
|
||||
|
||||
add_compile_options(#-Wall
|
||||
-O0
|
||||
add_compile_options(-Wall
|
||||
-Ofast
|
||||
-g3
|
||||
-Wno-format # int != int32 _t as far as the compiler is concerned because gcc has int32_t as long int
|
||||
-Wno-unused-function # we have some for the docs that aren't called
|
||||
-Wno-maybe-uninitialized)
|
||||
@@ -66,15 +71,15 @@ pico_set_program_version(RP2040-Decoder "1.0")
|
||||
# - Be cautious: Logging can alter timing behavior. For example, the PID controller is time critical,
|
||||
# limit the frequency of log outputs—such as logging only every 100th iteration.
|
||||
# - Note that with UART logging enabled, PICO_DEFAULT_UART_TX_PIN and PICO_DEFAULT_UART_RX_PIN are disabled for decoder functions.
|
||||
# These pins are used for UART communication. Disablign is done automatically in RP2040-Decoder-board.h or RP2040-Decoder-board-legacy.h respectively.
|
||||
# These pins are used for UART communication. Disabling is done automatically in RP2040-Decoder-board.h or RP2040-Decoder-board-legacy.h respectively.
|
||||
set(LOGLEVEL 1)
|
||||
# Additionally, the LOG_WAIT parameter can be configured to make the controller wait for user input via stdio before continuing.
|
||||
# This is useful for ensuring that all stdio logging outputs are captured.
|
||||
# If disabled, there may not be enough time to connect the serial terminal before most of the logging has already been completed.
|
||||
set(LOG_WAIT 1)
|
||||
set(LOG_WAIT 0)
|
||||
# Modify the below lines to enable/disable output over UART and/or USB
|
||||
set(STDIO_USB_ENABLED 1)
|
||||
set(STDIO_UART_ENABLED 1)
|
||||
set(STDIO_UART_ENABLED 0)
|
||||
# Leave those two unmodified
|
||||
pico_enable_stdio_uart(RP2040-Decoder ${STDIO_UART_ENABLED})
|
||||
pico_enable_stdio_usb(RP2040-Decoder ${STDIO_USB_ENABLED})
|
||||
@@ -102,7 +107,8 @@ target_link_libraries( RP2040-Decoder
|
||||
hardware_pwm
|
||||
hardware_adc
|
||||
hardware_flash
|
||||
hardware_watchdog )
|
||||
hardware_watchdog
|
||||
hardware_exception )
|
||||
|
||||
# Add the standard include files to the build
|
||||
target_include_directories(RP2040-Decoder PRIVATE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
@@ -48,8 +48,10 @@
|
||||
// Offset from base address used for saving CV_ARRAY_FLASH - in this case one sector size from the end of flash
|
||||
#define FLASH_TARGET_OFFSET (PICO_FLASH_SIZE_BYTES-FLASH_SECTOR_SIZE)
|
||||
|
||||
// For some reason the clock divider of 4 is necessary, otherwise the controller occasionally hard faults
|
||||
// Another workaround could be to run code from RAM but this seems to work fine
|
||||
#ifndef PICO_FLASH_SPI_CLKDIV
|
||||
#define PICO_FLASH_SPI_CLKDIV 2
|
||||
#define PICO_FLASH_SPI_CLKDIV 4
|
||||
#endif
|
||||
|
||||
|
||||
@@ -64,6 +66,8 @@
|
||||
#define MOTOR_REV_PIN 22u
|
||||
#define FWD_V_EMF_ADC_PIN 28u
|
||||
#define REV_V_EMF_ADC_PIN 29u
|
||||
#define FWD_V_EMF_ADC_CHANNEL (FWD_V_EMF_ADC_PIN-26u)
|
||||
#define REV_V_EMF_ADC_CHANNEL (REV_V_EMF_ADC_PIN-26u)
|
||||
|
||||
#if LOGLEVEL != 0
|
||||
// GPIO used directly (GPIO 2-5 incl.) as outputs or to switch auxiliary output transistors (GPIO 24-27 incl.)
|
||||
|
||||
@@ -51,8 +51,10 @@
|
||||
// Offset from base address used for saving CV_ARRAY_FLASH - in this case one sector size from the end of flash
|
||||
#define FLASH_TARGET_OFFSET (PICO_FLASH_SIZE_BYTES-FLASH_SECTOR_SIZE)
|
||||
|
||||
// For some reason the clock divider of 4 is necessary, otherwise the controller occasionally hard faults
|
||||
// Another workaround could be to run code from RAM but this seems to work fine
|
||||
#ifndef PICO_FLASH_SPI_CLKDIV
|
||||
#define PICO_FLASH_SPI_CLKDIV 2
|
||||
#define PICO_FLASH_SPI_CLKDIV 4
|
||||
#endif
|
||||
|
||||
|
||||
@@ -67,6 +69,8 @@
|
||||
#define MOTOR_REV_PIN 22u
|
||||
#define FWD_V_EMF_ADC_PIN 29u
|
||||
#define REV_V_EMF_ADC_PIN 28u
|
||||
#define FWD_V_EMF_ADC_CHANNEL (FWD_V_EMF_ADC_PIN-26u)
|
||||
#define REV_V_EMF_ADC_CHANNEL (REV_V_EMF_ADC_PIN-26u)
|
||||
|
||||
|
||||
#if LOGLEVEL != 0
|
||||
|
||||
+262
-191
@@ -6,37 +6,75 @@
|
||||
|
||||
#include "core0.h"
|
||||
|
||||
// XIP_BASE + FLASH_TARGET_OFFSET = "0x101FF000" <- This address need to be smaller than __flash_binary_end stored in RP2040-Decoder.elf.map
|
||||
// Define pointer to CV array stored in flash memory, FLASH_TARGET_OFFSET is defined in CMakeLists.txt and depending on flash size,
|
||||
// it gets stored in the highest address flash sector possible, to avoid interference with the program code.
|
||||
// CMakelists.txt also checks for program code exceeding past the last sector. Just have to make sure the correct flash size is set in CMakeLists.txt
|
||||
const uint8_t *CV_ARRAY_FLASH = (const uint8_t *) (XIP_BASE + FLASH_TARGET_OFFSET);
|
||||
uint64_t input_bit_buffer = 1;
|
||||
uint16_t level_table[32] = {0};
|
||||
absolute_time_t falling_edge_time, rising_edge_time;
|
||||
|
||||
// level_table is used to store pwm levels for any output using PWM
|
||||
uint16_t level_table[sizeof(uint32_t)*8] = {0};
|
||||
|
||||
// Volatile due to modification in gpio interrupt context
|
||||
volatile absolute_time_t falling_edge_time, rising_edge_time;
|
||||
|
||||
// Input bit buffer for DCC signal
|
||||
uint64_t input_bit_buffer = 0;
|
||||
dcc_ring_buffer_t dcc_r_buf = {0};
|
||||
|
||||
|
||||
// Function to read the flash size of the chip
|
||||
uint32_t get_flash_size() {
|
||||
// Prepare TX and RX buffers
|
||||
uint8_t txbuf[4] = { FLASH_CMD_READ_JEDEC_ID, 0, 0, 0 }; // Send the 0x9F command
|
||||
uint8_t rxbuf[4] = { 0, 0, 0, 0 }; // RX buffer for the response
|
||||
static void increment_ring_buffer_idx(size_t * idx, uint8_t ring_buffer_length) {
|
||||
// Takes index variable as a pointer and increments it by 1. When index reaches the end of the ring buffer, it wraps around.
|
||||
// The wrap around value is the length of the ring buffer.
|
||||
*idx = (*idx + 1) % ring_buffer_length;
|
||||
}
|
||||
|
||||
// Read the JEDEC ID
|
||||
flash_do_cmd(txbuf, rxbuf, sizeof(txbuf));
|
||||
static size_t get_flash_size() {
|
||||
// Function to read the flash size of the chip, used for debugging/testing purposes
|
||||
// Prepare TX and RX buffers for reading the JEDEC ID
|
||||
uint8_t tx_buf[4] = { FLASH_CMD_READ_JEDEC_ID, 0, 0, 0 }; // Send the 0x9F command
|
||||
uint8_t rx_buf[4] = { 0, 0, 0, 0 }; // RX buffer for the response
|
||||
size_t buf_size = sizeof(tx_buf);
|
||||
void * params[] = {tx_buf, rx_buf, &buf_size};
|
||||
int ret_val = flash_safe_execute(call_flash_do_cmd, params, FLASH_TIMEOUT_IN_MS);
|
||||
if (ret_val != PICO_OK){
|
||||
set_error(FLASH_SIZE_READBACK_FAILURE);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// The third byte of the response (rxbuf[3]) contains the log2 size (e.g., 0x18 for 16 MB)
|
||||
if (rxbuf[3] == 0) {
|
||||
if (rx_buf[3] == 0) {
|
||||
set_error(FLASH_SIZE_READBACK_FAILURE);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Calculate the actual size (2^size_code bytes)
|
||||
uint32_t flash_size = (1u<<rxbuf[3]);
|
||||
|
||||
size_t flash_size = (1u<<rx_buf[3]);
|
||||
|
||||
// Return the flash size in bytes
|
||||
return flash_size;
|
||||
}
|
||||
|
||||
// This function will be called when it's safe to call flash_range_erase
|
||||
// Always erases one sector, see FLASH_SECTOR_SIZE
|
||||
void hardfault_handler(void) {
|
||||
// Custom HardFault handler
|
||||
// Log hardfault
|
||||
LOG(1, "HARDFAULT @ core%u!\n", get_core_num());
|
||||
LOG(1, "Rebooting...\n")
|
||||
|
||||
// Hardfault can't be recovered, trigger the watchdog to reset the system
|
||||
watchdog_reboot(0, 0, 1);
|
||||
}
|
||||
|
||||
static void trigger_hardfault(void) {
|
||||
// Force a hardfault, used for debugging/testing purposes
|
||||
__asm volatile("UDF #0"); // Undefined instruction
|
||||
}
|
||||
|
||||
static void call_flash_range_erase(void *param) {
|
||||
// This function will be called when it's safe to call flash_range_erase
|
||||
// Always erases one sector, see FLASH_SECTOR_SIZE
|
||||
// Flash can only be erased in sectors, which effectively sets all bits to 1
|
||||
// Flash can than be programmed by setting bits to 0
|
||||
// Extract the parameters
|
||||
uint32_t offset = ((uintptr_t*)param)[0];
|
||||
uint32_t byte_count = ((uintptr_t*)param)[1];
|
||||
// Check for offset alignment (Must be aligned to a 4096-byte flash sector)
|
||||
@@ -48,8 +86,9 @@ static void call_flash_range_erase(void *param) {
|
||||
flash_range_erase(offset, byte_count);
|
||||
}
|
||||
|
||||
// This function will be called when it's safe to call flash_range_program
|
||||
static void call_flash_range_program(void *param) {
|
||||
// This function will be called when it's safe to call flash_range_program
|
||||
// Extract the parameters
|
||||
uint32_t offset = ((uintptr_t*)param)[0];
|
||||
uint32_t byte_count = ((uintptr_t*)param)[1];
|
||||
const uint8_t *data = (const uint8_t *)((uintptr_t*)param)[2];
|
||||
@@ -62,14 +101,24 @@ static void call_flash_range_program(void *param) {
|
||||
flash_range_program(offset, data, byte_count);
|
||||
}
|
||||
|
||||
unsigned int abs_val(int x) {
|
||||
// Vorzeichenbit extrahieren (falls das höchstwertige Bit 1 ist, ist die Zahl negativ)
|
||||
int mask = x >> (sizeof(x) * 8 - 1); // Rechtsverschiebung des Vorzeichens
|
||||
printf("%d\n", mask);
|
||||
static void call_flash_do_cmd(void *param) {
|
||||
// This function will be called when it's safe to call flash_do_cmd
|
||||
// Cast void pointer to void pointer array
|
||||
void **params = (void **)param;
|
||||
// Extract the parameters
|
||||
uint8_t * tx_buf = (uint8_t *)params[0];
|
||||
uint8_t * rx_buf = (uint8_t *)params[1];
|
||||
uint8_t * size_ptr = (uint8_t *)params[2];
|
||||
flash_do_cmd(tx_buf, rx_buf, *size_ptr);
|
||||
}
|
||||
|
||||
unsigned int absolute_val(int x) {
|
||||
// Extract the sign bit
|
||||
int mask = x >> (sizeof(x) * 8 - 1);
|
||||
return (x + mask) ^ mask;
|
||||
}
|
||||
|
||||
uint16_t two_std_dev(const uint16_t arr[], const uint32_t length) {
|
||||
static uint16_t two_std_dev(const uint16_t arr[], const uint32_t length) {
|
||||
// Calculate arithmetic average
|
||||
uint64_t sum = 0;
|
||||
for (uint32_t i = 0; i < length; ++i) {
|
||||
@@ -89,7 +138,7 @@ uint16_t two_std_dev(const uint16_t arr[], const uint32_t length) {
|
||||
sum = 0;
|
||||
uint32_t counter = 0;
|
||||
for (uint32_t i = 0; i < length; ++i) {
|
||||
const uint32_t diff = abs_val(arr[i] - x_avg);
|
||||
const uint32_t diff = absolute_val(arr[i] - x_avg);
|
||||
if ( diff <= 2*std_dev ){
|
||||
sum += arr[i];
|
||||
counter++;
|
||||
@@ -98,12 +147,10 @@ uint16_t two_std_dev(const uint16_t arr[], const uint32_t length) {
|
||||
return sum / counter;
|
||||
}
|
||||
|
||||
|
||||
// Measures constant adc offset and programs the offset into flash
|
||||
// TODO: check necessity and correctness of adc offset function
|
||||
void adc_offset_adjustment(const uint32_t n) {
|
||||
static void adc_offset_adjustment(uint32_t const n) {
|
||||
// Measures constant adc offset and programs the offset into flash
|
||||
LOG(1, "ADC offset adjustment...\n");
|
||||
|
||||
|
||||
uint16_t offsets_fwd[n];
|
||||
uint16_t offsets_rev[n];
|
||||
// Set PWM level to 0 just to be sure
|
||||
@@ -111,24 +158,30 @@ void adc_offset_adjustment(const uint32_t n) {
|
||||
pwm_set_gpio_level(MOTOR_REV_PIN, 0);
|
||||
busy_wait_ms(1000);
|
||||
|
||||
// Measure n times in "reverse direction" (GPIO 28)
|
||||
adc_select_input(2); //TODO: "2" as input argument?
|
||||
|
||||
// Measure n times in (reverse direction)
|
||||
LOG(1, "Measuring ADC offset in reverse direction (input channel: %u)...\n", REV_V_EMF_ADC_CHANNEL);
|
||||
adc_select_input(REV_V_EMF_ADC_CHANNEL);
|
||||
adc_run(true);
|
||||
for (uint32_t i = 0; i < n; i++) {
|
||||
offsets_rev[i] = adc_fifo_get_blocking();
|
||||
}
|
||||
adc_run(false);
|
||||
adc_fifo_drain();
|
||||
LOG(1, "Done measuring in reverse direction...\n");
|
||||
busy_wait_ms(1000);
|
||||
|
||||
// Measure n times in "forward direction" (GPIO 29)
|
||||
adc_select_input(3);
|
||||
|
||||
// Measure n times in (forward direction)
|
||||
LOG(1, "Measuring ADC offset in forward direction (input channel: %u)...\n", FWD_V_EMF_ADC_CHANNEL);
|
||||
adc_select_input(FWD_V_EMF_ADC_CHANNEL);
|
||||
adc_run(true);
|
||||
for (uint32_t i = 0; i < n; i++) {
|
||||
offsets_fwd[i] = adc_fifo_get_blocking();
|
||||
}
|
||||
adc_run(false);
|
||||
adc_fifo_drain();
|
||||
LOG(1, "Done measuring in forward direction...\n");
|
||||
|
||||
// Find overall average discarding outliers in measurement
|
||||
const uint16_t offset_avg_fwd = two_std_dev(offsets_fwd, n);
|
||||
@@ -142,21 +195,21 @@ void adc_offset_adjustment(const uint32_t n) {
|
||||
memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP));
|
||||
CV_ARRAY_TEMP[171] = overall_avg_offset;
|
||||
uintptr_t params_erase[] = {FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE};
|
||||
int ret_val = flash_safe_execute(call_flash_range_erase, params_erase, UINT32_MAX);
|
||||
int ret_val = flash_safe_execute(call_flash_range_erase, params_erase, FLASH_TIMEOUT_IN_MS);
|
||||
if (ret_val != PICO_OK){
|
||||
set_error(FLASH_SAFE_EXECUTE_ERASE_FAILURE);
|
||||
return;
|
||||
}
|
||||
uintptr_t params_program[] = {FLASH_TARGET_OFFSET, sizeof(CV_ARRAY_TEMP), (uintptr_t)CV_ARRAY_TEMP};
|
||||
ret_val = flash_safe_execute(call_flash_range_program, params_program, UINT32_MAX);
|
||||
ret_val = flash_safe_execute(call_flash_range_program, params_program, FLASH_TIMEOUT_IN_MS);
|
||||
if(ret_val != PICO_OK){
|
||||
set_error(FLASH_SAFE_EXECUTE_PROGRAM_FAILURE);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void acknowledge() {
|
||||
static void acknowledge() {
|
||||
// Acknowledge a CV rd/wr instruction by pulsing the motor in both directions
|
||||
const uint16_t max_lvl = _125M / (CV_ARRAY_FLASH[8] * 100 + 10000);
|
||||
pwm_set_gpio_level(MOTOR_FWD_PIN, max_lvl);
|
||||
busy_wait_ms(3);
|
||||
@@ -166,8 +219,7 @@ void acknowledge() {
|
||||
pwm_set_gpio_level(MOTOR_REV_PIN, 0);
|
||||
}
|
||||
|
||||
|
||||
void verify_cv_bit(const uint16_t cv_address, const bool bit_val, const uint8_t bit_pos) {
|
||||
static void verify_cv_bit(uint16_t const cv_address, const bool bit_val, uint8_t const bit_pos) {
|
||||
// Check for matching bit, when found call acknowledge()
|
||||
const uint8_t mask = 0b00000001;
|
||||
const bool res = ((CV_ARRAY_FLASH[cv_address] >> bit_pos) & mask) == bit_val;
|
||||
@@ -176,26 +228,25 @@ void verify_cv_bit(const uint16_t cv_address, const bool bit_val, const uint8_t
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void verify_cv_byte(const uint16_t cv_address, const uint8_t cv_data) {
|
||||
static void verify_cv_byte(uint16_t const cv_address, uint8_t const cv_data) {
|
||||
// Check for matching byte, when found call acknowledge()
|
||||
if (CV_ARRAY_FLASH[cv_address] == cv_data) acknowledge();
|
||||
}
|
||||
|
||||
|
||||
void write_cv_byte(uint16_t cv_address, uint8_t cv_data) {
|
||||
static void write_cv_byte(uint16_t cv_address, uint8_t cv_data) {
|
||||
// Write the specified CV byte to flash
|
||||
uint8_t CV_ARRAY_TEMP[CV_ARRAY_SIZE];
|
||||
memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP));
|
||||
CV_ARRAY_TEMP[cv_address] = cv_data;
|
||||
// First erase necessary amount of blocks in flash and then rewrite to flash
|
||||
uintptr_t params_erase[] = {FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE};
|
||||
int ret_val = flash_safe_execute(call_flash_range_erase, params_erase, UINT32_MAX);
|
||||
int ret_val = flash_safe_execute(call_flash_range_erase, params_erase, FLASH_TIMEOUT_IN_MS);
|
||||
if (ret_val != PICO_OK){
|
||||
set_error(FLASH_SAFE_EXECUTE_ERASE_FAILURE);
|
||||
return;
|
||||
}
|
||||
uintptr_t params_program[] = { FLASH_TARGET_OFFSET, sizeof(CV_ARRAY_TEMP), (uintptr_t)CV_ARRAY_TEMP};
|
||||
ret_val = flash_safe_execute(call_flash_range_program, params_program, UINT32_MAX);
|
||||
ret_val = flash_safe_execute(call_flash_range_program, params_program, FLASH_TIMEOUT_IN_MS);
|
||||
if(ret_val != PICO_OK){
|
||||
set_error(FLASH_SAFE_EXECUTE_PROGRAM_FAILURE);
|
||||
return;
|
||||
@@ -203,23 +254,24 @@ void write_cv_byte(uint16_t cv_address, uint8_t cv_data) {
|
||||
acknowledge();
|
||||
}
|
||||
|
||||
void reset_cv_array_to_default(){
|
||||
static void reset_cv_array_to_default(){
|
||||
// Reset all CVs to default (CV_ARRAY_DEFAULT)
|
||||
uintptr_t params_erase[] = {FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE};
|
||||
int ret_val = flash_safe_execute(call_flash_range_erase, params_erase, UINT32_MAX);
|
||||
int ret_val = flash_safe_execute(call_flash_range_erase, params_erase, FLASH_TIMEOUT_IN_MS);
|
||||
if (ret_val != PICO_OK){
|
||||
set_error(FLASH_SAFE_EXECUTE_ERASE_FAILURE);
|
||||
return;
|
||||
}
|
||||
uintptr_t params_program[] = {FLASH_TARGET_OFFSET, sizeof(CV_ARRAY_DEFAULT), (uintptr_t)CV_ARRAY_DEFAULT};
|
||||
ret_val = flash_safe_execute(call_flash_range_program, params_program, UINT32_MAX);
|
||||
ret_val = flash_safe_execute(call_flash_range_program, params_program, FLASH_TIMEOUT_IN_MS);
|
||||
if(ret_val != PICO_OK){
|
||||
set_error(FLASH_SAFE_EXECUTE_PROGRAM_FAILURE);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// There are some exceptions to writing CVs - e.g. writing to a read only CV is not valid - this gets handled here
|
||||
void write_cv_handler(const uint16_t cv_index, const uint8_t cv_data) {
|
||||
static void write_cv_handler(uint16_t const cv_index, uint8_t const cv_data) {
|
||||
// There are some exceptions to writing CVs - e.g. writing to a read only CV is not valid - this gets handled here
|
||||
switch (cv_index) {
|
||||
case 0: //CV_1
|
||||
// CV_1 => Value = 0 is not allowed
|
||||
@@ -246,32 +298,30 @@ void write_cv_handler(const uint16_t cv_index, const uint8_t cv_data) {
|
||||
reset_cv_array_to_default();
|
||||
}
|
||||
break;
|
||||
// CV_17 must have a value between 11000000->(192dec) and 11100111->(231dec) inclusive
|
||||
// CV_17 == 192 && CV_18 == 0 -> Address = 0 is also not valid and therefore shall not be set
|
||||
case 16: //CV_17
|
||||
case 16: // CV_17
|
||||
// CV_17 must have a value between 11000000->(192dec) and 11100111->(231dec) inclusive
|
||||
// CV_17 == 192 && CV_18 == 0 -> Address = 0 is also not valid and therefore shall not be set
|
||||
if ((cv_data < 192 || cv_data > 231) || (CV_ARRAY_FLASH[17] == 0 && cv_data == 192)) {
|
||||
break;
|
||||
}
|
||||
write_cv_byte(cv_index, cv_data);
|
||||
break;
|
||||
case 17: //CV_18
|
||||
case 17: // CV_18
|
||||
if(CV_ARRAY_FLASH[16] == 192 && cv_data == 0){
|
||||
break;
|
||||
}
|
||||
write_cv_byte(cv_index, cv_data);
|
||||
break;
|
||||
// CV_31 & CV_32 Index high byte isn't implemented and shall not be set
|
||||
case 30: //CV_31
|
||||
case 31: //CV_32
|
||||
case 30: // CV_31 & CV_32 Index isn't implemented and shall not be set
|
||||
break;
|
||||
// Regular CV write is carried out
|
||||
default:
|
||||
case 31: // CV_31 & CV_32 Index isn't implemented and shall not be set
|
||||
break;
|
||||
default: // Regular CV write is carried out
|
||||
write_cv_byte(cv_index, cv_data);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void program_mode(const uint8_t number_of_bytes, const uint8_t *const byte_array) {
|
||||
static void program_mode(size_t const number_of_bytes, const uint8_t *const byte_array) {
|
||||
//First check for valid programming command ("address" 112-127)
|
||||
if (byte_array[number_of_bytes - 1] < 128 && byte_array[number_of_bytes - 1] > 111) {
|
||||
const uint8_t instruction_type_mask = 0b00001100;
|
||||
@@ -305,8 +355,7 @@ void program_mode(const uint8_t number_of_bytes, const uint8_t *const byte_array
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void set_outputs(const uint32_t functions_to_set_bitmask) {
|
||||
static void set_outputs(uint32_t const functions_to_set_bitmask) {
|
||||
// Get outputs with pwm enabled and preset outputs_to_set_PWM variable with resulting GPIO Bitmask
|
||||
const uint32_t PWM_enabled_outputs = get_32bit_CV(111);
|
||||
uint32_t outputs_to_set_PWM = PWM_enabled_outputs;
|
||||
@@ -342,35 +391,34 @@ void set_outputs(const uint32_t functions_to_set_bitmask) {
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Update functions F0 - F31 when function command is received or in case of direction change
|
||||
void update_active_functions(uint32_t new_function_bitmask, const uint8_t clr_bit_ind, const bool direction_change) {
|
||||
static void update_active_functions(uint32_t new_function_bitmask, const uint8_t clr_bit_ind, bool const direction_change) {
|
||||
// Update functions F0 - F31 when function command is received or in case of direction change
|
||||
static uint32_t active_functions;
|
||||
static uint32_t prev_active_func;
|
||||
|
||||
// Outputs depend on direction, therefore on direction change outputs need to be set as configured for the other direction.
|
||||
// Outputs depend on direction, therefore on direction change outputs need to be set for the other direction.
|
||||
if (direction_change) {
|
||||
// In case of a direction change every output needs to be set according to configuration for the other direction
|
||||
set_outputs(active_functions);
|
||||
}
|
||||
else {
|
||||
// TODO: improve explanation here...
|
||||
// In case of a change in active functions only the function group corresponding to the instruction is considered
|
||||
// Check for changes and set outputs accordingly
|
||||
// In case of a change in active functions only the function group corresponding to the instruction is considered for updating the outputs
|
||||
// First, check for changes
|
||||
new_function_bitmask &= ~(clr_bit_arr[clr_bit_ind]); // Clear every bit except bits corresponding to group of functions
|
||||
active_functions &= clr_bit_arr[clr_bit_ind]; // Clear bits corresponding to group of functions
|
||||
active_functions |= new_function_bitmask; // Write bits corresponding to group of functions
|
||||
const uint32_t changes = active_functions ^ prev_active_func; // Checks for changes from previous state
|
||||
// When there is changes, set outputs accordingly
|
||||
if (changes) {
|
||||
set_outputs(active_functions);
|
||||
}
|
||||
// Update previously active function variable
|
||||
prev_active_func = active_functions;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Bitwise XOR for all Bytes -> when result is: "0b00000000" error check is passed.
|
||||
bool error_detection(const int8_t number_of_bytes, const uint8_t *const byte_array) {
|
||||
static bool error_detection(size_t const number_of_bytes, const uint8_t *const byte_array) {
|
||||
// Bitwise XOR for all Bytes -> when result is: "0b00000000" error check is passed.
|
||||
uint8_t xor_byte = 0;
|
||||
for (int i = 0; i < number_of_bytes; i++) {
|
||||
xor_byte = xor_byte ^ byte_array[i];
|
||||
@@ -378,18 +426,16 @@ bool error_detection(const int8_t number_of_bytes, const uint8_t *const byte_arr
|
||||
return (0 == xor_byte);
|
||||
}
|
||||
|
||||
|
||||
// Check for long address. Function returns true for long address.
|
||||
bool is_long_address(uint8_t const number_of_bytes, const uint8_t *const byte_array) {
|
||||
static bool is_long_address(size_t const number_of_bytes, const uint8_t *const byte_array) {
|
||||
// Check for long address. Function returns true for long address, false for short address.
|
||||
if ((byte_array[number_of_bytes - 1] >> 6) == 0b00000011) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
// First checks for idle message and then evaluates address included in byte_array[] which contains the received DCC message
|
||||
bool address_evaluation(const uint8_t number_of_bytes, const uint8_t *const byte_array) {
|
||||
static bool address_evaluation(size_t const number_of_bytes, const uint8_t *const byte_array) {
|
||||
// First checks for idle message and then evaluates address included in byte_array[] which contains the received DCC message
|
||||
// Check for idle message
|
||||
if (byte_array[number_of_bytes - 1] == 255) {
|
||||
return false;
|
||||
@@ -424,9 +470,8 @@ bool address_evaluation(const uint8_t number_of_bytes, const uint8_t *const byte
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Evaluate the type of instruction
|
||||
void instruction_evaluation(const uint8_t number_of_bytes, const uint8_t *const byte_array) {
|
||||
static void instruction_evaluation(size_t const number_of_bytes, const uint8_t *const byte_array) {
|
||||
// Evaluate the type of instruction
|
||||
// start of transmission -> ... -> command_byte_n -> ... -> command_byte_0 -> ... -> end of transmission
|
||||
// The position of command bytes depend on whether the address is long or not
|
||||
uint8_t command_byte_start_index;
|
||||
@@ -467,7 +512,7 @@ void instruction_evaluation(const uint8_t number_of_bytes, const uint8_t *const
|
||||
}
|
||||
|
||||
else if (command_byte_n >> 5 == 0b00000110) {
|
||||
//Feature Expansion Instruction 110X-XXXX
|
||||
// Feature Expansion Instruction 110X-XXXX
|
||||
switch (command_byte_n) {
|
||||
case 0b11011110: // F13-F20
|
||||
update_active_functions((byte_array[command_byte_start_index - 1] << 13), 3, false);
|
||||
@@ -484,16 +529,14 @@ void instruction_evaluation(const uint8_t number_of_bytes, const uint8_t *const
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Check for reset message - When reset message is found, stop the motor and disable all functions.
|
||||
bool reset_message_check(const uint8_t number_of_bytes, const uint8_t *const byte_array) {
|
||||
static bool reset_message_check(size_t const number_of_bytes, const uint8_t *const byte_array) {
|
||||
// Check for reset message - When reset message is found, stop the motor and disable all functions.
|
||||
if (byte_array[number_of_bytes - 1] == 0b00000000 && byte_array[number_of_bytes - 2] == 0b00000000) {
|
||||
const direction_t dir = get_direction_of_speed_step(speed_step_target);
|
||||
// Set previous speed step target to current speed step target
|
||||
speed_step_target_prev = speed_step_target;
|
||||
// TODO: Improve comment below
|
||||
// Set speed step target to emergency stop speed step ( 1 for Reverse direction / 129 for forward direction)
|
||||
// for details see get_direction_of_speed_step() doxygen comment in "shared.h"
|
||||
// for details see get_direction_of_speed_step() comment in "shared.h"
|
||||
speed_step_target = 1 + 128*dir;
|
||||
update_active_functions(0,0,0); // Disables all functions
|
||||
return true;
|
||||
@@ -501,11 +544,11 @@ bool reset_message_check(const uint8_t number_of_bytes, const uint8_t *const byt
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
// Checks for valid message - looking for correct preamble using bitmasks
|
||||
// returns number of bytes if valid bit-pattern is found. Otherwise -1 is returned.
|
||||
int8_t verify_dcc_message() {
|
||||
int8_t number_of_bytes = -1;
|
||||
static size_t detect_dcc_packet() {
|
||||
// Checks for valid package - looking for correct preamble using bitmasks
|
||||
// returns number of bytes if valid bit-pattern is found. Otherwise INVALID_PACKAGE is returned.
|
||||
// INVALID_PACKAGE is defined in core0.h as SIZE_MAX (maximum value of size_t)
|
||||
size_t number_of_bytes = INVALID_PACKAGE;
|
||||
const uint64_t masked_message_3_bytes = input_bit_buffer & MESSAGE_MASK_3_BYTES;
|
||||
if (masked_message_3_bytes == MESSAGE_3_BYTES) number_of_bytes = 3;
|
||||
const uint64_t masked_message_4_bytes = input_bit_buffer & MESSAGE_MASK_4_BYTES;
|
||||
@@ -515,58 +558,59 @@ int8_t verify_dcc_message() {
|
||||
return number_of_bytes;
|
||||
}
|
||||
|
||||
|
||||
//start of transmission -> byte_n(address byte) -> ... -> byte_0(error detection byte) -> end of transmission
|
||||
void bits_to_byte_array(const int8_t number_of_bytes, uint8_t byte_array[]) {
|
||||
for (uint8_t i = 0; i < number_of_bytes; i++) {
|
||||
byte_array[i] = input_bit_buffer >> (i * 9 + 1);
|
||||
static void bits_to_dcc_packet_data(dcc_packet_t * packet) {
|
||||
// Writes data in input_bit_buffer to dcc_packet_t struct instance
|
||||
//start of transmission -> byte_n(address byte) -> ... -> byte_0(error detection byte) -> end of transmission
|
||||
for (uint8_t i = 0; i < packet->length; i++) {
|
||||
packet->data[i] = input_bit_buffer >> (i * 9 + 1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Sequence of bits gets evaluated
|
||||
void evaluate_message() {
|
||||
static void evaluate_packet(dcc_packet_t * packet) {
|
||||
// DCC packet evaluation
|
||||
static bool reset_message_flag;
|
||||
// Check if input buffer contains a valid dcc message
|
||||
const int8_t number_of_bytes = verify_dcc_message();
|
||||
// number_of_bytes contains the amount of bytes when the message is valid, otherwise -1
|
||||
if (number_of_bytes != -1) {
|
||||
// Split data into array of bytes
|
||||
uint8_t byte_array[SIZE_BYTE_ARRAY] = {0};
|
||||
bits_to_byte_array(number_of_bytes, byte_array);
|
||||
// Check for errors
|
||||
if (error_detection(number_of_bytes, byte_array)) {
|
||||
// Check for matching address
|
||||
if (address_evaluation(number_of_bytes, byte_array)) {
|
||||
reset_message_flag = false;
|
||||
instruction_evaluation(number_of_bytes, byte_array);
|
||||
}
|
||||
else if (reset_message_flag) {
|
||||
// When previous packet was a reset message -> enter program mode
|
||||
program_mode(number_of_bytes, byte_array);
|
||||
}
|
||||
else {
|
||||
// Check for reset message and set flag when reset message is received
|
||||
reset_message_flag = reset_message_check(number_of_bytes, byte_array);
|
||||
}
|
||||
// Check for errors
|
||||
if (error_detection(packet->length, packet->data)) {
|
||||
// Check for matching address
|
||||
if (address_evaluation(packet->length, packet->data)) {
|
||||
reset_message_flag = false;
|
||||
instruction_evaluation(packet->length, packet->data);
|
||||
}
|
||||
else if (reset_message_flag) {
|
||||
// When previous packet was a reset message -> enter program mode
|
||||
program_mode(packet->length, packet->data);
|
||||
}
|
||||
else {
|
||||
// Check for reset message and set flag when reset message is received
|
||||
reset_message_flag = reset_message_check(packet->length, packet->data);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// DCC-Signal (GPIO 21) rising edge interrupt
|
||||
void track_signal_rise(const unsigned int gpio, const long unsigned int events) {
|
||||
// Save current timer value
|
||||
rising_edge_time = get_absolute_time();
|
||||
// Disable rising edge interrupt and enable falling edge interrupt
|
||||
gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_RISE, false, &track_signal_rise);
|
||||
gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_FALL, true, &track_signal_fall);
|
||||
static void gpio_irq_cb(uint gpio, uint32_t events) {
|
||||
absolute_time_t start_time = get_absolute_time();
|
||||
// Check for rising edge on DCC input pin
|
||||
if ((events & GPIO_IRQ_EDGE_RISE) && (gpio == DCC_INPUT_PIN)) {
|
||||
track_signal_rise(gpio, events);
|
||||
}
|
||||
// Check for falling edge on DCC input pin
|
||||
else if ((events & GPIO_IRQ_EDGE_FALL) && (gpio == DCC_INPUT_PIN)) {
|
||||
track_signal_fall(gpio, events);
|
||||
}
|
||||
absolute_time_t end_time = get_absolute_time();
|
||||
if (absolute_time_diff_us(start_time, end_time) > 20) {
|
||||
LOG(1, "Time gpio_irq_cb: %lld us\n", absolute_time_diff_us(start_time, end_time));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void track_signal_rise() {
|
||||
// DCC-Signal pin rising edge. Write current time to rising_edge_time
|
||||
rising_edge_time = get_absolute_time();
|
||||
}
|
||||
|
||||
// DCC-Signal (GPIO 21) falling edge interrupt
|
||||
void track_signal_fall( const unsigned int gpio, const long unsigned int events) {
|
||||
// Save current timer value
|
||||
static void track_signal_fall() {
|
||||
// DCC-Signal pin falling edge. Write current time to falling_edge_time
|
||||
falling_edge_time = get_absolute_time();
|
||||
// Time difference is equal to the time the signal was in a logical high state
|
||||
const int64_t time_logical_high = absolute_time_diff_us(rising_edge_time, falling_edge_time);
|
||||
@@ -576,20 +620,24 @@ void track_signal_fall( const unsigned int gpio, const long unsigned int events)
|
||||
else bit = 1;
|
||||
input_bit_buffer <<= 1;
|
||||
input_bit_buffer |= bit;
|
||||
// evaluate sequence of bits saved in buffer
|
||||
evaluate_message();
|
||||
// Disable falling edge interrupt and enable rising edge interrupt
|
||||
gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_FALL, false, &track_signal_fall);
|
||||
gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_RISE, true, &track_signal_rise);
|
||||
// Check if input buffer contains a valid dcc packet
|
||||
// number_of_bytes contains the length of the packet when detected, otherwise -1
|
||||
dcc_r_buf.packets[dcc_r_buf.wr_idx].length = detect_dcc_packet();
|
||||
if (dcc_r_buf.packets[dcc_r_buf.wr_idx].length != INVALID_PACKAGE) {
|
||||
// Write data into ring buffer containing dcc_packet_t struct instances
|
||||
bits_to_dcc_packet_data(&dcc_r_buf.packets[dcc_r_buf.wr_idx]);
|
||||
// Increment write index of ring buffer
|
||||
increment_ring_buffer_idx(&dcc_r_buf.wr_idx, RING_BUFFER_PACKETS);
|
||||
}
|
||||
}
|
||||
|
||||
void init_outputs() {
|
||||
static void init_outputs() {
|
||||
LOG(1, "Initializing Outputs...\n");
|
||||
gpio_init_mask(GPIO_ALLOWED_OUTPUTS);
|
||||
gpio_set_dir_out_masked(GPIO_ALLOWED_OUTPUTS); // Note: This might also disable UART on GPIO0 & GPIO1
|
||||
const uint32_t PWM_enabled_outputs = get_32bit_CV(111) & GPIO_ALLOWED_OUTPUTS;
|
||||
uint32_t mask = 1;
|
||||
for (uint8_t i = 0; i < 32; ++i) {
|
||||
for (uint8_t i = 0; i < sizeof(uint32_t)*8; ++i) {
|
||||
if (PWM_enabled_outputs & mask) {
|
||||
const uint32_t slice = pwm_gpio_to_slice_num(i);
|
||||
const uint32_t channel = pwm_gpio_to_channel(i); // Channel A = "0"; Channel B = "1"
|
||||
@@ -608,25 +656,24 @@ void init_outputs() {
|
||||
}
|
||||
}
|
||||
|
||||
void cv_setup_check() {
|
||||
static void cv_setup_check() {
|
||||
LOG(1, "Checking CV array for factory state of flash or missing ADC offset setup...\n");
|
||||
// Check for flash factory setting and set CV_FLASH_ARRAY to default values when factory condition ("0xFF") is found.
|
||||
if (CV_ARRAY_FLASH[64] == 0xFF) {
|
||||
LOG(1, "Detected flash memory factory condidition (CV_65 == 0xFF), resetting all CVs to default values...\n");
|
||||
LOG(1, "Detected flash memory factory condidition (CV_65 == %u), resetting all CVs to default values...\n", CV_ARRAY_FLASH[64]);
|
||||
reset_cv_array_to_default();
|
||||
}
|
||||
|
||||
// Check for existing ADC offset setup
|
||||
if (CV_ARRAY_FLASH[171] == 0xFF) {
|
||||
LOG(1, "Detected ADC offset factory condidition (CV_172 == 0xFF), running offset adjustment measurement function...\n");
|
||||
LOG(1, "Detected ADC offset factory condidition (CV_172 == %u), running offset adjustment measurement function...\n", CV_ARRAY_FLASH[171]);
|
||||
adc_offset_adjustment(ADC_CALIBRATION_ITERATIONS);
|
||||
}
|
||||
LOG(1, "CV check done!\n");
|
||||
}
|
||||
|
||||
|
||||
// Motor PWM initialization
|
||||
void init_motor_pwm(const uint8_t gpio) {
|
||||
static void init_motor_pwm(uint8_t const gpio) {
|
||||
// Motor PWM initialization
|
||||
LOG(2, "Initializing Motor PWM on GPIO pin: %u...\n", gpio);
|
||||
// Set GPIO pin to PWM functionality
|
||||
gpio_set_function(gpio, GPIO_FUNC_PWM);
|
||||
@@ -642,7 +689,7 @@ void init_motor_pwm(const uint8_t gpio) {
|
||||
LOG(2, "Initialized motor PWM on pin: %u; wrap_counter %u; clkdiv %d;\n", gpio, wrap_counter, CV_ARRAY_FLASH[173]);
|
||||
}
|
||||
|
||||
void init_adc() {
|
||||
static void init_adc() {
|
||||
LOG(1, "Initializing ADC...\n")
|
||||
// Initialize ADC
|
||||
adc_init();
|
||||
@@ -654,22 +701,21 @@ void init_adc() {
|
||||
LOG(1, "ADC initialization done!\n")
|
||||
}
|
||||
|
||||
void init_digital_inputs() {
|
||||
LOG(1, "Initializing digital inputs and track signal irq...\n");
|
||||
static void init_digital_input() {
|
||||
LOG(1, "Initializing DCC input pin...\n");
|
||||
gpio_init(DCC_INPUT_PIN);
|
||||
gpio_set_dir(DCC_INPUT_PIN, GPIO_IN);
|
||||
gpio_pull_up(DCC_INPUT_PIN);
|
||||
gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_RISE, true, &track_signal_rise);
|
||||
LOG(1, "Digital inputs and track signal irq initialization done!\n");
|
||||
LOG(1, "DCC input pin initialization done!\n");
|
||||
}
|
||||
|
||||
void wait_for_input() {
|
||||
static void wait_for_stdio_input() {
|
||||
while (true) {
|
||||
watchdog_update();
|
||||
//watchdog_update();
|
||||
// Send a message to the user asking them to acknowledge it by sending a reply
|
||||
LOG(1, "Send any character to continue.\n");
|
||||
// Wait for a character
|
||||
int received_char = getchar_timeout_us((WATCHDOG_TIMER_IN_MS/2)*1000);
|
||||
int received_char = getchar_timeout_us(1e6);
|
||||
if ((received_char != PICO_ERROR_TIMEOUT) && (received_char > 0)) {
|
||||
// If a character is received, exit the function
|
||||
LOG(1, "Character '%c' received. Continuing...\n", (char)received_char);
|
||||
@@ -679,45 +725,42 @@ void wait_for_input() {
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int main() {
|
||||
// Initialize LED GPIO pin
|
||||
gpio_init(RP2040_DECODER_DEFAULT_LED_PIN);
|
||||
gpio_set_dir(RP2040_DECODER_DEFAULT_LED_PIN, GPIO_OUT);
|
||||
exception_set_exclusive_handler(HARDFAULT_EXCEPTION, hardfault_handler);
|
||||
|
||||
// Initialize stdio
|
||||
if(stdio_init_all() != true){
|
||||
set_error(STDIO_INIT_FAILURE);
|
||||
}
|
||||
stdio_flush();
|
||||
|
||||
// Wait for user input before continuing, this is only enabled when LOG_WAIT is set to 1 and logging is enabled (LOGLEVEL > 0)
|
||||
// Also disabled when no STDIO is configured as the user would not be able to send anything
|
||||
# if (LOGLEVEL > 0 && LOG_WAIT == 1) && (STDIO_UART_ENABLED || STDIO_USB_ENABLED)
|
||||
wait_for_input();
|
||||
printf("%u", STDIO_UART_ENABLED);
|
||||
#ifdef RP2040_DECODER_DEFAULT_LED_PIN
|
||||
// Initialize LED GPIO pin
|
||||
gpio_init(RP2040_DECODER_DEFAULT_LED_PIN);
|
||||
gpio_set_dir(RP2040_DECODER_DEFAULT_LED_PIN, GPIO_OUT);
|
||||
// Blink LED once for 200ms to indicate startup
|
||||
gpio_put(RP2040_DECODER_DEFAULT_LED_PIN, 1);
|
||||
busy_wait_ms(200);
|
||||
gpio_put(RP2040_DECODER_DEFAULT_LED_PIN, 0);
|
||||
busy_wait_ms(200);
|
||||
#endif
|
||||
|
||||
// Initialize stdio when enabled
|
||||
#if (STDIO_UART_ENABLED || STDIO_USB_ENABLED)
|
||||
if(stdio_init_all() != true){
|
||||
set_error(STDIO_INIT_FAILURE);
|
||||
}
|
||||
// When LOG_WAIT == 1 and LOGLEVEL > 0 wait for user input before continuing. User input can be any character.
|
||||
#if (LOGLEVEL > 0 && LOG_WAIT == 1)
|
||||
wait_for_stdio_input();
|
||||
printf("%u", STDIO_UART_ENABLED);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
LOG(1, "core0 Initialization...\n");
|
||||
|
||||
// Launch core1
|
||||
multicore_launch_core1(core1_entry);
|
||||
|
||||
// Wait 10ms to give core1 some time to call flash_safe_execute_core_init()
|
||||
busy_wait_ms(10);
|
||||
|
||||
// Check for reboot by watchdog and set error when true
|
||||
if (watchdog_caused_reboot()) {
|
||||
set_error(REBOOT_BY_WATCHDOG);
|
||||
}
|
||||
|
||||
// Enable the watchdog, requiring the watchdog to be updated every 2000ms or the chip will reboot
|
||||
|
||||
// Enable the watchdog, requiring the watchdog to be updated every WATCHDOG_TIMER_IN_MS milliseconds or the chip will reboot
|
||||
// second arg is pause on debug which means the watchdog will pause when stepping through code
|
||||
watchdog_enable(WATCHDOG_TIMER_IN_MS, 1);
|
||||
|
||||
// Check for error calling flash_safe_execute_core_init() on core1
|
||||
if(get_error_state() & FLASH_SAFE_EXECUTE_CORE_INIT_FAILURE){
|
||||
panic("Error calling flash_safe_execute_core_init() on core1!");
|
||||
// Check for reboot by watchdog and set error when true
|
||||
if (watchdog_caused_reboot()) {
|
||||
LOG(1, "Reboot by watchdog detected!\n");
|
||||
set_error(REBOOT_BY_WATCHDOG);
|
||||
}
|
||||
|
||||
// Initialize Motor PWM pins
|
||||
@@ -730,17 +773,45 @@ int main() {
|
||||
// Initialize ADC
|
||||
init_adc();
|
||||
|
||||
// Check CV array for factory state of flash or missing ADC offset setup
|
||||
cv_setup_check();
|
||||
|
||||
// Initialize digital inputs and track signal irq
|
||||
init_digital_inputs();
|
||||
init_digital_input();
|
||||
|
||||
LOG(1, "core0 initialization done!\n");
|
||||
|
||||
// Launch core1
|
||||
multicore_launch_core1(core1_entry);
|
||||
// Wait 100ms to give core1 some time to call flash_safe_execute_core_init()
|
||||
busy_wait_ms(100);
|
||||
|
||||
|
||||
// Check for error calling flash_safe_execute_core_init() on core1
|
||||
if(get_error_state() & FLASH_SAFE_EXECUTE_CORE_INIT_FAILURE){
|
||||
panic("Error calling flash_safe_execute_core_init() on core1!"); //TODO: test
|
||||
}
|
||||
|
||||
// Check CV array for factory state of flash or missing ADC offset setup
|
||||
cv_setup_check();
|
||||
|
||||
// Register GPIO IRQ callback
|
||||
gpio_set_irq_callback(gpio_irq_cb);
|
||||
// Enable IRQ
|
||||
gpio_set_irq_enabled(DCC_INPUT_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
|
||||
irq_set_enabled(IO_IRQ_BANK0, true);
|
||||
|
||||
// Endless loop
|
||||
while (true) {
|
||||
sleep_ms(400);
|
||||
watchdog_update();
|
||||
// Check for new messages in ring buffer
|
||||
if (dcc_r_buf.wr_idx != dcc_r_buf.rd_idx) {
|
||||
absolute_time_t start_time = get_absolute_time();
|
||||
evaluate_packet(&dcc_r_buf.packets[dcc_r_buf.rd_idx]);
|
||||
increment_ring_buffer_idx(&dcc_r_buf.rd_idx, RING_BUFFER_PACKETS);
|
||||
absolute_time_t end_time = get_absolute_time();
|
||||
if (absolute_time_diff_us(start_time, end_time) > 40) {
|
||||
LOG(1, "Time to evaluate message: %lld us\n", absolute_time_diff_us(start_time, end_time));
|
||||
}
|
||||
}
|
||||
else {
|
||||
watchdog_update();
|
||||
}
|
||||
}
|
||||
}
|
||||
+167
-58
@@ -9,7 +9,6 @@
|
||||
#include "shared.h"
|
||||
#include "CV.h"
|
||||
|
||||
#define SIZE_BYTE_ARRAY 5
|
||||
#define MESSAGE_3_BYTES 0b11111111110000000000000000000000000001
|
||||
#define MESSAGE_MASK_3_BYTES 0b11111111111000000001000000001000000001
|
||||
#define MESSAGE_4_BYTES 0b11111111110000000000000000000000000000000000001
|
||||
@@ -17,9 +16,16 @@
|
||||
#define MESSAGE_5_BYTES 0b11111111110000000000000000000000000000000000000000000001
|
||||
#define MESSAGE_MASK_5_BYTES 0b11111111111000000001000000001000000001000000001000000001
|
||||
|
||||
#define INVALID_PACKAGE SIZE_MAX
|
||||
|
||||
#define RING_BUFFER_PACKETS 8
|
||||
#define RING_BUFFER_BYTES 5
|
||||
|
||||
#define FLASH_CMD_READ_JEDEC_ID 0x9F
|
||||
#define FLASH_TIMEOUT_IN_MS 500
|
||||
|
||||
#define ADC_CALIBRATION_ITERATIONS 8192
|
||||
#define WATCHDOG_TIMER_IN_MS 2000
|
||||
#define WATCHDOG_TIMER_IN_MS 5000
|
||||
|
||||
/*!
|
||||
* \brief Bitmasks used for clearing bits corresponding to function blocks
|
||||
@@ -41,29 +47,126 @@ const uint32_t clr_bit_arr[6] = {
|
||||
0b00011111111111111111111111111111, // Clear Bits 29-31 (F29-F31)
|
||||
};
|
||||
|
||||
/*!
|
||||
* \brief Function returns average of values deviating less than twice the standard deviation from original average.
|
||||
*
|
||||
* \param arr input array
|
||||
* \param length amount of elements in input array
|
||||
* \return average value of values deviating less than 2*standard_dev.
|
||||
|
||||
/**
|
||||
* @brief Structure representing a DCC packet.
|
||||
*
|
||||
* This structure is used to store the data and length of a DCC packet.
|
||||
*
|
||||
* @typedef dcc_packet_t
|
||||
*
|
||||
* @field data An array of bytes representing the DCC packet data.
|
||||
* @field length The length of the DCC packet data.
|
||||
*/
|
||||
uint16_t two_std_dev(const uint16_t arr[], uint32_t length);
|
||||
typedef struct dcc_packet_t {
|
||||
uint8_t data[RING_BUFFER_BYTES];
|
||||
size_t length;
|
||||
}dcc_packet_t;
|
||||
|
||||
/**
|
||||
* @brief Structure representing a ring buffer for DCC packets.
|
||||
*
|
||||
* This structure is used to manage a ring buffer that stores DCC packets.
|
||||
* It contains an array of packets and indices for writing and reading.
|
||||
*
|
||||
* @typedef dcc_ring_buffer_t
|
||||
*
|
||||
* @field packets Array of DCC packets stored in the ring buffer.
|
||||
* @field wr_idx Index for the next write operation in the ring buffer.
|
||||
* @field rd_idx Index for the next read operation in the ring buffer.
|
||||
*/
|
||||
typedef struct dcc_ring_buffer_t {
|
||||
dcc_packet_t packets[RING_BUFFER_PACKETS];
|
||||
size_t wr_idx;
|
||||
size_t rd_idx;
|
||||
} dcc_ring_buffer_t;
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Increment the index of a ring buffer.
|
||||
*
|
||||
* This function increments the provided index of a ring buffer, wrapping around
|
||||
* to zero if the end of the buffer is reached.
|
||||
*
|
||||
* @param idx Pointer to the index to be incremented.
|
||||
* @param ring_buffer_length The length of the ring buffer.
|
||||
*/
|
||||
static void increment_ring_buffer_idx(size_t * idx, uint8_t ring_buffer_length);
|
||||
|
||||
/*!
|
||||
* \brief Function for erasing a sector of flash memory.
|
||||
*
|
||||
* This function erases a specified range of flash memory sectors.
|
||||
*
|
||||
* \param param Pointer to parameters.
|
||||
* - First parameter: Offset into flash, in bytes. Must be aligned to a 4096-byte flash sector.
|
||||
* - Second parameter: Number of bytes to be erased. Must be a multiple of 4096 bytes (one sector).
|
||||
*/
|
||||
static void call_flash_range_erase(void *param);
|
||||
|
||||
/*!
|
||||
* \brief Function for programming a range of flash memory
|
||||
*
|
||||
* This function programs a specified range of flash memory with the provided data.
|
||||
*
|
||||
* \param param Pointer to parameters
|
||||
* - First parameter: The Flash address of the first byte to be programmed. Must be aligned to a 256-byte flash page.
|
||||
* - Second parameter: The number of bytes to program. Must be a multiple of 256 bytes (one page).
|
||||
* - Third parameter: Pointer to the data to program into flash.
|
||||
*/
|
||||
static void call_flash_range_program(void *param);
|
||||
|
||||
/*!
|
||||
* \brief Function for executing a flash command via QSPI
|
||||
*
|
||||
* \param param Pointer to parameters
|
||||
* First parameter is the pointer to a byte buffer which will be transmitted to the flash
|
||||
* Second parameter is the pointer to a byte buffer where data received from the flash will be written.
|
||||
* Third parameter is the length in bytes of any of the two buffers (same size).
|
||||
*/
|
||||
static void call_flash_do_cmd(void *param);
|
||||
|
||||
/*!
|
||||
* \brief Function for calculating the absolute value of an integer.
|
||||
*
|
||||
* This function takes an integer as input and returns its absolute value.
|
||||
*
|
||||
* \param x The input integer whose absolute value is to be calculated.
|
||||
* \return The absolute value of the input integer.
|
||||
*/
|
||||
static unsigned int absolute_val(int x);
|
||||
|
||||
/*!
|
||||
* \brief Computes the average of values in an array that deviate less than twice the standard deviation from the original average.
|
||||
*
|
||||
* This function calculates the average of the elements in the input array that are within two standard deviations of the mean of the array.
|
||||
*
|
||||
* \param arr The input array of values.
|
||||
* \param length The number of elements in the input array.
|
||||
* \return The average value of the elements that deviate less than twice the standard deviation from the original average.
|
||||
*/
|
||||
static uint16_t two_std_dev(const uint16_t arr[], uint32_t length);
|
||||
|
||||
/*!
|
||||
* \brief Measures ADC offset and saves the offset value to flash memory.
|
||||
* \param n Amount of measurements to be done
|
||||
*
|
||||
* This function performs a series of ADC measurements to determine the offset
|
||||
* and then stores this offset value in the flash memory for future use.
|
||||
*
|
||||
* \param n The number of measurements to be performed to calculate the offset.
|
||||
*/
|
||||
void adc_offset_adjustment(uint32_t n);
|
||||
static void adc_offset_adjustment(uint32_t n);
|
||||
|
||||
/*!
|
||||
* \brief Function for acknowledging a CV write or CV verify instruction
|
||||
*
|
||||
* Acknowledgement works by creating a spike in current consumption, normally realized by running the motor in alternating directions.
|
||||
* This current increase is detected by the command station.
|
||||
* Refers to NMRA S-9.2.3 Section D and RCN-216 Section 3
|
||||
*
|
||||
* \note Refers to NMRA S-9.2.3 Section D and RCN-216 Section 3
|
||||
*/
|
||||
void acknowledge();
|
||||
static void acknowledge();
|
||||
|
||||
/*!
|
||||
* \brief Function for verifying a single bit of a CV
|
||||
@@ -71,39 +174,48 @@ void acknowledge();
|
||||
* \param bit_val Value of bit to be verified
|
||||
* \param bit_pos Position of bit to be verified
|
||||
*/
|
||||
void verify_cv_bit(uint16_t cv_address, bool bit_val, uint8_t bit_pos);
|
||||
static void verify_cv_bit(uint16_t cv_address, bool bit_val, uint8_t bit_pos);
|
||||
|
||||
/*!
|
||||
* \brief Function for verifying a CV byte
|
||||
* \param cv_address CV Address
|
||||
* \param cv_data Byte to be verified
|
||||
*
|
||||
* This function verifies the byte of a Configuration Variable (CV) at the specified address.
|
||||
*
|
||||
* \param cv_address The address of the CV to be verified.
|
||||
* \param cv_data The byte value to be verified against the CV.
|
||||
*/
|
||||
void verify_cv_byte(uint16_t cv_address, uint8_t cv_data);
|
||||
static void verify_cv_byte(uint16_t cv_address, uint8_t cv_data);
|
||||
|
||||
/*!
|
||||
* \brief Function for writing a CV byte
|
||||
* \param cv_address
|
||||
* \param cv_data
|
||||
* \brief Writes a byte to a Configuration Variable (CV).
|
||||
*
|
||||
* This function writes a byte of data to a specified CV address.
|
||||
*
|
||||
* \param cv_address The address of the CV to write to.
|
||||
* \param cv_data The data byte to write to the CV.
|
||||
*/
|
||||
void write_cv_byte(uint16_t cv_address, uint8_t cv_data);
|
||||
static void write_cv_byte(uint16_t cv_address, uint8_t cv_data);
|
||||
|
||||
/*!
|
||||
* \brief This function sets all the CV values stored in flash back to their default settings (CV_ARRAY_DEFAULT).
|
||||
*/
|
||||
void reset_cv_array_to_default();
|
||||
static void reset_cv_array_to_default();
|
||||
|
||||
/*!
|
||||
* \brief CV Programming mode function
|
||||
* Used on entering programming mode in order to write CV values to persistent flash memory or verify/read bits/bytes from flash. <br>
|
||||
* Refers to NMRA Standard 9.2.3 ("Service Mode Programming") or RCN-216<br>
|
||||
*
|
||||
* This function is used to enter programming mode in order to write CV values to persistent flash memory or verify/read bits/bytes from flash memory.
|
||||
*
|
||||
* Procedure:
|
||||
* 1. Checks for valid programming command
|
||||
* 2. Evaluates the type of programming instruction Write byte / Verify byte / Verify bit and runs corresponding function
|
||||
* 1. Checks for a valid programming command.
|
||||
* 2. Evaluates the type of programming instruction (Write byte / Verify byte / Verify bit) and runs the corresponding function.
|
||||
*
|
||||
* \param number_of_bytes Number of bytes in the array.
|
||||
* \param byte_array Pointer to the byte array.
|
||||
* @param number_of_bytes The number of bytes in the array.
|
||||
* @param byte_array A pointer to the byte array.
|
||||
*
|
||||
* @note Refer to NMRA Standard 9.2.3 ("Service Mode Programming") or RCN-216 for more details.
|
||||
*/
|
||||
void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
static void program_mode(size_t number_of_bytes, const uint8_t *const byte_array);
|
||||
|
||||
/*!
|
||||
* \brief Set outputs according to function mapping in CV257 to CV512
|
||||
@@ -113,16 +225,19 @@ void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
*
|
||||
* \param functions_to_set_bitmask Function bitmask F0 is bit0, F1 is bit1, ...
|
||||
*/
|
||||
void set_outputs(uint32_t functions_to_set_bitmask);
|
||||
static void set_outputs(uint32_t functions_to_set_bitmask);
|
||||
|
||||
/**
|
||||
* @brief Update functions F0 - F31 when function command is received or in case of direction change.
|
||||
*
|
||||
* @param new_function_bitmask New function bitmask.
|
||||
* @param clr_bit_ind Clear bit index.
|
||||
* @param direction_change Direction change flag.
|
||||
* This function updates the active functions based on the provided bitmask. It can also clear a specific bit
|
||||
* and handle changes in direction.
|
||||
*
|
||||
* @param new_function_bitmask New function bitmask representing the state of functions F0 - F31.
|
||||
* @param clr_bit_ind Index of the bit to be cleared in the bitmask.
|
||||
* @param direction_change Flag indicating if there is a change in direction.
|
||||
*/
|
||||
void update_active_functions(uint32_t new_function_bitmask, uint8_t clr_bit_ind, bool direction_change);
|
||||
static void update_active_functions(uint32_t new_function_bitmask, uint8_t clr_bit_ind, bool direction_change);
|
||||
|
||||
/**
|
||||
* @brief Detect errors in the byte array using exor of all bytes.
|
||||
@@ -133,7 +248,7 @@ void update_active_functions(uint32_t new_function_bitmask, uint8_t clr_bit_ind,
|
||||
* @param byte_array Pointer to the byte array.
|
||||
* @return true if no error is detected, false otherwise.
|
||||
*/
|
||||
bool error_detection(int8_t number_of_bytes, const uint8_t *byte_array);
|
||||
static bool error_detection(size_t number_of_bytes, const uint8_t *byte_array);
|
||||
|
||||
/**
|
||||
* @brief Check if the address is a long address.
|
||||
@@ -142,7 +257,7 @@ bool error_detection(int8_t number_of_bytes, const uint8_t *byte_array);
|
||||
* @param byte_array Pointer to the byte array.
|
||||
* @return true if it is a long address, false otherwise.
|
||||
*/
|
||||
bool is_long_address(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
static bool is_long_address(size_t number_of_bytes, const uint8_t byte_array[]);
|
||||
|
||||
/**
|
||||
* @brief Evaluate the address of the message.
|
||||
@@ -151,7 +266,7 @@ bool is_long_address(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
* @param byte_array Pointer to the byte array.
|
||||
* @return true if the address is correct, false otherwise.
|
||||
*/
|
||||
bool address_evaluation(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
static bool address_evaluation(size_t number_of_bytes, const uint8_t byte_array[]);
|
||||
|
||||
/**
|
||||
* @brief Evaluate the instruction of the message.
|
||||
@@ -159,14 +274,14 @@ bool address_evaluation(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
* @param number_of_bytes Number of bytes in the array.
|
||||
* @param byte_array Pointer to the byte array.
|
||||
*/
|
||||
void instruction_evaluation(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
static void instruction_evaluation(size_t number_of_bytes, const uint8_t byte_array[]);
|
||||
|
||||
/**
|
||||
* @brief Check for reset message - When reset message is found, stop the motor and disable all functions.
|
||||
*
|
||||
* @return Number of bytes if valid reset message bit-pattern is found. Otherwise -1 is returned.
|
||||
*/
|
||||
bool reset_message_check(uint8_t number_of_bytes, const uint8_t *const byte_array);
|
||||
static bool reset_message_check(size_t number_of_bytes, const uint8_t *const byte_array);
|
||||
|
||||
/**
|
||||
* @brief Verify DCC message
|
||||
@@ -177,7 +292,7 @@ bool reset_message_check(uint8_t number_of_bytes, const uint8_t *const byte_arra
|
||||
*
|
||||
* \return Number of bytes if valid bit-pattern is found. Otherwise return -1.
|
||||
*/
|
||||
int8_t verify_dcc_message();
|
||||
static size_t detect_dcc_packet();
|
||||
|
||||
/**
|
||||
* @brief Convert sequence of bits to byte array
|
||||
@@ -186,10 +301,9 @@ int8_t verify_dcc_message();
|
||||
* Bytes are ordered as follows:
|
||||
* Start of transmission -> byte_n(address byte) -> ... -> byte_0(error detection byte) -> end of transmission
|
||||
*
|
||||
* \param number_of_bytes Number of bytes in the array.
|
||||
* \param byte_array Pointer to the byte array.
|
||||
* \param packet Pointer to dcc packet struct which contains data and length of packet.
|
||||
*/
|
||||
void bits_to_byte_array(int8_t number_of_bytes, uint8_t byte_array[]);
|
||||
static void bits_to_dcc_packet_data(dcc_packet_t * packet);
|
||||
|
||||
/**
|
||||
* @brief Main message evaluation function
|
||||
@@ -202,32 +316,27 @@ void bits_to_byte_array(int8_t number_of_bytes, uint8_t byte_array[]);
|
||||
* 5. Check for reset message and set flag when reset message is received
|
||||
*
|
||||
*/
|
||||
void evaluate_message();
|
||||
static void evaluate_packet();
|
||||
|
||||
/*!
|
||||
* \brief Track signal rising edge interrupt callback function
|
||||
*
|
||||
* Saves timer counter value using get_absolute_time(), then disables rising edge IRQ and enables falling edge IRQ.
|
||||
* Parameters are unused.
|
||||
*
|
||||
* \param gpio Unused parameter
|
||||
* \param events Unused parameter
|
||||
*/
|
||||
void track_signal_rise(unsigned int gpio, long unsigned int events);
|
||||
static void track_signal_rise();
|
||||
|
||||
/*!
|
||||
* \brief Track signal falling edge interrupt callback function
|
||||
*
|
||||
* Saves timer counter value using get_absolute_time(), then calculates time difference between rising and falling edge.
|
||||
* When the time difference is greater than 87us, this bit is interpreted as a logical 0.
|
||||
* When the time difference is greater than 87us, this bit is interpreted as a logical 0. Otherwise, it is interpreted as a logical 1.
|
||||
* Refers to NMRA S-9.1 and RCN-210 standards
|
||||
* Puts the latest bit value into buffer, then evaluates complete buffer using evaluate_message().
|
||||
* Parameters are unused.
|
||||
* Puts the latest bit value into 64 bit unsigned integer "buffer", then checks if the buffer contains a valid DCC packet.
|
||||
* When a valid packet is found, it is written into the ring buffer, and the write index gets incremented.
|
||||
*
|
||||
* \param gpio Unused parameter
|
||||
* \param events Unused parameter
|
||||
*/
|
||||
void track_signal_fall(unsigned int gpio, long unsigned int events);
|
||||
static void track_signal_fall();
|
||||
|
||||
/*!
|
||||
* \brief Output initialization function
|
||||
@@ -236,7 +345,7 @@ void track_signal_fall(unsigned int gpio, long unsigned int events);
|
||||
* Sets PWM functionality for GPIOs configured as PWM pins CV_112 - CV_115 (see CV.h for reference) <br>
|
||||
* PWM frequency and clock divider are configured according to CV_116 - CV_171 (see CV.h for reference) <br>
|
||||
*/
|
||||
void init_outputs();
|
||||
static void init_outputs();
|
||||
|
||||
/*!
|
||||
* \brief Checks various CV values in order to detect factory condition or pending calibrations
|
||||
@@ -246,17 +355,17 @@ void init_outputs();
|
||||
* 2. Check for existing ADC offset setup when not found write 7 to read-only CV_7 in order to trigger a ADC offset measurement/calibration
|
||||
* 3. Check for base PWM configuration/calibration, run calibration when none is found
|
||||
*/
|
||||
void cv_setup_check();
|
||||
static void cv_setup_check();
|
||||
|
||||
/*!
|
||||
* \brief Function initializes motor PWM for pin specified in CMakeLists.txt
|
||||
* \param gpio GPIO pin
|
||||
*/
|
||||
void init_motor_pwm(uint8_t gpio);
|
||||
static void init_motor_pwm(uint8_t gpio);
|
||||
|
||||
/*!
|
||||
* \brief Initializes ADC
|
||||
*
|
||||
* Function initializes ADC, corresponding GPIO pins specified in CMakeLists.txt, also configures ADC FIFO.
|
||||
*/
|
||||
void init_adc();
|
||||
static void init_adc();
|
||||
+49
-19
@@ -7,6 +7,7 @@
|
||||
#include "core1.h"
|
||||
|
||||
struct repeating_timer pid_control_timer, speed_helper_timer;
|
||||
bool controller_flag, speed_helper_flag = false;
|
||||
|
||||
|
||||
// Measures Back-EMF voltage (proportional to the rotational speed of the motor) on GPIO 28 and GPIO 29 respectively (depending on direction)
|
||||
@@ -14,14 +15,20 @@ float measure(uint8_t total_iterations,
|
||||
uint8_t measurement_delay_us,
|
||||
uint8_t l_side_arr_cutoff,
|
||||
uint8_t r_side_arr_cutoff,
|
||||
bool direction){ //TODO: Change to direction_t
|
||||
direction_t direction){
|
||||
uint32_t sum = 0;
|
||||
uint16_t adc_val[total_iterations];
|
||||
int32_t j = 0;
|
||||
uint16_t key;
|
||||
pwm_set_gpio_level(MOTOR_FWD_PIN, 0);
|
||||
pwm_set_gpio_level(MOTOR_REV_PIN, 0);
|
||||
adc_select_input((!direction)+2); // TODO: fix with #define macros!
|
||||
if (direction == DIRECTION_FORWARD) {
|
||||
adc_select_input(FWD_V_EMF_ADC_CHANNEL);
|
||||
} else if(direction == DIRECTION_REVERSE) {
|
||||
adc_select_input(REV_V_EMF_ADC_CHANNEL);
|
||||
} else{
|
||||
set_error(INVALID_DIRECTION);
|
||||
}
|
||||
busy_wait_us(measurement_delay_us);
|
||||
adc_run(true);
|
||||
for (int32_t i = 0; i < total_iterations; ++i) {
|
||||
@@ -36,6 +43,7 @@ float measure(uint8_t total_iterations,
|
||||
adc_val[j + 1] = key;
|
||||
}
|
||||
adc_run(false);
|
||||
//adc_fifo_drain();
|
||||
//discard x entries beginning from the lowest entry of the array (counting up); x = msr.l_side_arr_cutoff
|
||||
//discard y entries beginning from the highest index of the array (counting down); y = msr.r_side_arr_cutoff
|
||||
for (int32_t i = l_side_arr_cutoff; i < total_iterations - r_side_arr_cutoff ; ++i) {
|
||||
@@ -61,10 +69,9 @@ uint8_t get_speed_step_table_index_of_speed_step(uint8_t speed_step) {
|
||||
|
||||
// This function gets called every x milliseconds where x is CV_175.
|
||||
// The purpose of this function is to implement a time delay in acceleration or deceleration
|
||||
bool speed_helper(struct repeating_timer *const t) {
|
||||
void speed_helper(controller_parameter_t * ctrl_par) {
|
||||
// ctrl_par->setpoint only gets adjusted when the speed_helper_counter is equal to the accel_rate/decel_rate
|
||||
// -> Time for 1 Speed Step := (speed_helper timer delay)*(accel_rate) or CV_175*CV_3 or CV_175*CV_4
|
||||
controller_parameter_t *const ctrl_par = (controller_parameter_t *) (t->user_data);
|
||||
const uint8_t accel_rate = CV_ARRAY_FLASH[2];
|
||||
const uint8_t decel_rate = CV_ARRAY_FLASH[3];
|
||||
static uint8_t speed_helper_counter;
|
||||
@@ -109,9 +116,6 @@ bool speed_helper(struct repeating_timer *const t) {
|
||||
// Target reached -> Reset Counter
|
||||
speed_helper_counter = 0;
|
||||
}
|
||||
|
||||
// return true to schedule the repeating timer again
|
||||
return true;
|
||||
}
|
||||
|
||||
// Helper function to adjust pwm level/duty cycle.
|
||||
@@ -209,14 +213,15 @@ void controller_pid_mode(controller_parameter_t *const ctrl_par) {
|
||||
if (LOGLEVEL >= 1) {
|
||||
static int counter = 0;
|
||||
counter++;
|
||||
if (counter > 100) {
|
||||
if (counter > 200) {
|
||||
LOG(1, "ctrl_par error(%f) output(%f)\n", ctrl_par->pid.e, output_f);
|
||||
counter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Set PWM Level and discard results in adc fifo
|
||||
adjust_pwm_level((uint16_t) roundf(output_f));
|
||||
//adjust_pwm_level((uint16_t) roundf(output_f));
|
||||
adjust_pwm_level((uint16_t) output_f);
|
||||
// Save previous error, integrator, differentiator values
|
||||
ctrl_par->pid.e_prev = ctrl_par->pid.e;
|
||||
ctrl_par->pid.i_prev = i;
|
||||
@@ -225,10 +230,7 @@ void controller_pid_mode(controller_parameter_t *const ctrl_par) {
|
||||
|
||||
// General controller function gets called every x milliseconds where x is CV_49 i.e. sampling time (pid->t)
|
||||
// TODO: Consider fixed point arithmetic to improve performance...
|
||||
bool controller_general(struct repeating_timer *const t) {
|
||||
// Cast (void *) to (controller_parameter_t *)
|
||||
controller_parameter_t *const ctrl_par = (controller_parameter_t *) (t->user_data);
|
||||
|
||||
void controller_general(controller_parameter_t * ctrl_par) {
|
||||
// Change in direction -> reset previous derivative, error and integral parts and pwm_base_done
|
||||
const bool direction_changed = get_direction_of_speed_step(speed_step_target) !=
|
||||
get_direction_of_speed_step(speed_step_target_prev);
|
||||
@@ -240,8 +242,7 @@ bool controller_general(struct repeating_timer *const t) {
|
||||
ctrl_par->mode = STARTUP_MODE;
|
||||
ctrl_par->startup.level = 0;
|
||||
adjust_pwm_level(0);
|
||||
// return true exits and schedule the repeating timer again
|
||||
return true;
|
||||
return;
|
||||
}
|
||||
|
||||
// Measure BEMF voltage and compute error
|
||||
@@ -266,9 +267,6 @@ bool controller_general(struct repeating_timer *const t) {
|
||||
|
||||
// Save measurement value
|
||||
ctrl_par->measurement_prev = ctrl_par->measurement;
|
||||
|
||||
// return true to schedule the repeating timer again
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@@ -341,6 +339,17 @@ void init_controller(controller_parameter_t *const ctrl_par) {
|
||||
LOG(1, "Motor controller initialization done!\n")
|
||||
}
|
||||
|
||||
bool controller_timer_callback(__unused struct repeating_timer *t) {
|
||||
controller_flag = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool speed_helper_timer_callback(__unused struct repeating_timer *t) {
|
||||
speed_helper_flag = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void core1_entry() {
|
||||
LOG(1, "core1 Initialization...\n");
|
||||
|
||||
@@ -349,10 +358,13 @@ void core1_entry() {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
controller_parameter_t control_parameter;
|
||||
controller_parameter_t *ctrl_par = &control_parameter;
|
||||
init_controller(ctrl_par);
|
||||
|
||||
|
||||
/*
|
||||
alarm_pool_add_repeating_timer_ms(alarm_pool_create(0, 1),
|
||||
CV_ARRAY_FLASH[48],
|
||||
controller_general,
|
||||
@@ -366,10 +378,28 @@ void core1_entry() {
|
||||
ctrl_par,
|
||||
&speed_helper_timer);
|
||||
|
||||
*/
|
||||
|
||||
|
||||
struct repeating_timer timer_controller, timer_speed_helper;
|
||||
add_repeating_timer_ms(-CV_ARRAY_FLASH[48], controller_timer_callback, NULL, &timer_controller);
|
||||
add_repeating_timer_ms(-CV_ARRAY_FLASH[174], speed_helper_timer_callback, NULL, &timer_speed_helper);
|
||||
|
||||
LOG(1, "core1 initialization done!\n");
|
||||
|
||||
// Endless loop
|
||||
while (true) {
|
||||
tight_loop_contents();
|
||||
if (controller_flag) {
|
||||
controller_general(ctrl_par);
|
||||
controller_flag = false;
|
||||
}
|
||||
else if (speed_helper_flag) {
|
||||
speed_helper(ctrl_par);
|
||||
speed_helper_flag = false;
|
||||
}
|
||||
else {
|
||||
watchdog_update();
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
+6
-8
@@ -93,14 +93,14 @@ typedef struct controller_parameter_t{
|
||||
* @param measurement_delay_us The delay in microseconds between the switch-off of the motor PWM and the measurement.
|
||||
* @param l_side_arr_cutoff The number of lowest ADC values to discard.
|
||||
* @param r_side_arr_cutoff The number of highest ADC values to discard.
|
||||
* @param direction The direction of the motor. //TODO: Introduce Motor direction typedef
|
||||
* @param direction The direction of the motor.
|
||||
* @return The average of the middle ADC values after discarding the specified number of lowest and highest values.
|
||||
*/
|
||||
float measure(uint8_t total_iterations,
|
||||
uint8_t measurement_delay_us,
|
||||
uint8_t l_side_arr_cutoff,
|
||||
uint8_t r_side_arr_cutoff,
|
||||
bool direction);
|
||||
direction_t direction);
|
||||
|
||||
|
||||
/**
|
||||
@@ -116,10 +116,9 @@ uint8_t get_speed_step_table_index_of_speed_step(uint8_t speed_step);
|
||||
*
|
||||
* x is CV_175
|
||||
*
|
||||
* @param t Pointer to the repeating timer structure.
|
||||
* @return true to schedule the repeating timer again.
|
||||
* @param ctrl_par Pointer to the controller parameter structure.
|
||||
*/
|
||||
bool speed_helper(struct repeating_timer *t);
|
||||
void speed_helper(controller_parameter_t * ctrl_par);
|
||||
|
||||
/**
|
||||
* @brief Function to adjust the PWM level/duty cycle.
|
||||
@@ -170,10 +169,9 @@ void controller_pid_mode(controller_parameter_t *const ctrl_par);
|
||||
/**
|
||||
* @brief General controller function called every x milliseconds.
|
||||
*
|
||||
* @param t Pointer to the repeating timer structure.
|
||||
* @return true to schedule the repeating timer again.
|
||||
* @param ctrl_par Pointer to the controller parameter structure.
|
||||
*/
|
||||
bool controller_general(struct repeating_timer * t);
|
||||
void controller_general(controller_parameter_t * ctrl_par);
|
||||
|
||||
/**
|
||||
* @brief Initialize the all controller variables, measurement parameters, and speed table.
|
||||
|
||||
+19
-4
@@ -6,9 +6,10 @@
|
||||
|
||||
#include "shared.h"
|
||||
|
||||
// TODO: Implement enum typedef for speed steps, also getter and setter functions checking validity of speed step values.
|
||||
uint8_t speed_step_target = 0;
|
||||
uint8_t speed_step_target_prev = 0;
|
||||
uint32_t error_state = 0;
|
||||
static error_t error_state = 0;
|
||||
|
||||
// Functions in shared.c are accessed by both cores
|
||||
|
||||
@@ -36,13 +37,27 @@ direction_t get_direction_of_speed_step(uint8_t speed_step){
|
||||
|
||||
void set_error(error_t err){
|
||||
error_state |= err;
|
||||
gpio_put(RP2040_DECODER_DEFAULT_LED_PIN, true);
|
||||
#ifdef RP2040_DECODER_DEFAULT_LED_PIN
|
||||
gpio_put(RP2040_DECODER_DEFAULT_LED_PIN, true);
|
||||
#endif
|
||||
}
|
||||
|
||||
void clear_error(error_t err){
|
||||
error_state & (~err);
|
||||
error_state &= (~err);
|
||||
if (error_state == 0){
|
||||
#ifdef RP2040_DECODER_DEFAULT_LED_PIN
|
||||
gpio_put(RP2040_DECODER_DEFAULT_LED_PIN, false);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t get_error_state(){
|
||||
void clear_all_errors(){
|
||||
error_state = 0;
|
||||
#ifdef RP2040_DECODER_DEFAULT_LED_PIN
|
||||
gpio_put(RP2040_DECODER_DEFAULT_LED_PIN, false);
|
||||
#endif
|
||||
}
|
||||
|
||||
error_t get_error_state(){
|
||||
return error_state;
|
||||
}
|
||||
+273
-11
@@ -17,7 +17,17 @@
|
||||
#include "hardware/flash.h"
|
||||
#include "hardware/gpio.h"
|
||||
#include "hardware/watchdog.h"
|
||||
#include "hardware/exception.h"
|
||||
|
||||
#define LOG(level, ...) { \
|
||||
if (level <= LOGLEVEL) { \
|
||||
printf("[core%u @ %u ms] ", get_core_num(), to_ms_since_boot(get_absolute_time())); \
|
||||
printf(__VA_ARGS__); \
|
||||
} \
|
||||
}
|
||||
|
||||
// Constant Value of 125 x 10^6
|
||||
#define _125M 125000000
|
||||
|
||||
typedef enum {
|
||||
FLASH_SAFE_EXECUTE_CORE_INIT_FAILURE = (1<<0),
|
||||
@@ -26,6 +36,7 @@ typedef enum {
|
||||
FLASH_SIZE_READBACK_FAILURE = (1<<3),
|
||||
STDIO_INIT_FAILURE = (1<<4),
|
||||
REBOOT_BY_WATCHDOG = (1<<5),
|
||||
INVALID_DIRECTION = (1<<6),
|
||||
} error_t;
|
||||
|
||||
typedef enum {
|
||||
@@ -33,16 +44,266 @@ typedef enum {
|
||||
DIRECTION_REVERSE = 0,
|
||||
} direction_t;
|
||||
|
||||
//TODO: Continue implementation of speed step enum
|
||||
typedef enum {
|
||||
SPEED_STEP_REVERSE_STOP = 0x00, // 00000000bin 000dec
|
||||
SPEED_STEP_REVERSE_EMERGENCY_STOP = 0x01, // 00000001bin 001dec
|
||||
SPEED_STEP_REVERSE_001 = 0x02, // 00000010bin 002dec
|
||||
SPEED_STEP_REVERSE_002 = 0x03, // 00000011bin 003dec
|
||||
SPEED_STEP_REVERSE_003 = 0x04, // 00000100bin 004dec
|
||||
SPEED_STEP_REVERSE_004 = 0x05, // 00000101bin 005dec
|
||||
SPEED_STEP_REVERSE_005 = 0x06, // 00000110bin 006dec
|
||||
SPEED_STEP_REVERSE_006 = 0x07, // 00000111bin 007dec
|
||||
SPEED_STEP_REVERSE_007 = 0x08, // 00001000bin 008dec
|
||||
SPEED_STEP_REVERSE_008 = 0x09, // 00001001bin 009dec
|
||||
SPEED_STEP_REVERSE_009 = 0x0A, // 00001010bin 010dec
|
||||
SPEED_STEP_REVERSE_010 = 0x0B, // 00001011bin 011dec
|
||||
SPEED_STEP_REVERSE_011 = 0x0C, // 00001100bin 012dec
|
||||
SPEED_STEP_REVERSE_012 = 0x0D, // 00001101bin 013dec
|
||||
SPEED_STEP_REVERSE_013 = 0x0E, // 00001110bin 014dec
|
||||
SPEED_STEP_REVERSE_014 = 0x0F, // 00001111bin 015dec
|
||||
SPEED_STEP_REVERSE_015 = 0x10, // 00010000bin 016dec
|
||||
SPEED_STEP_REVERSE_016 = 0x11, // 00010001bin 017dec
|
||||
SPEED_STEP_REVERSE_017 = 0x12, // 00010010bin 018dec
|
||||
SPEED_STEP_REVERSE_018 = 0x13, // 00010011bin 019dec
|
||||
SPEED_STEP_REVERSE_019 = 0x14, // 00010100bin 020dec
|
||||
SPEED_STEP_REVERSE_020 = 0x15, // 00010101bin 021dec
|
||||
SPEED_STEP_REVERSE_021 = 0x16, // 00010110bin 022dec
|
||||
SPEED_STEP_REVERSE_022 = 0x17, // 00010111bin 023dec
|
||||
SPEED_STEP_REVERSE_023 = 0x18, // 00011000bin 024dec
|
||||
SPEED_STEP_REVERSE_024 = 0x19, // 00011001bin 025dec
|
||||
SPEED_STEP_REVERSE_025 = 0x1A, // 00011010bin 026dec
|
||||
SPEED_STEP_REVERSE_026 = 0x1B, // 00011011bin 027dec
|
||||
SPEED_STEP_REVERSE_027 = 0x1C, // 00011100bin 028dec
|
||||
SPEED_STEP_REVERSE_028 = 0x1D, // 00011101bin 029dec
|
||||
SPEED_STEP_REVERSE_029 = 0x1E, // 00011110bin 030dec
|
||||
SPEED_STEP_REVERSE_030 = 0x1F, // 00011111bin 031dec
|
||||
SPEED_STEP_REVERSE_031 = 0x20, // 00100000bin 032dec
|
||||
SPEED_STEP_REVERSE_032 = 0x21, // 00100001bin 033dec
|
||||
SPEED_STEP_REVERSE_033 = 0x22, // 00100010bin 034dec
|
||||
SPEED_STEP_REVERSE_034 = 0x23, // 00100011bin 035dec
|
||||
SPEED_STEP_REVERSE_035 = 0x24, // 00100100bin 036dec
|
||||
SPEED_STEP_REVERSE_036 = 0x25, // 00100101bin 037dec
|
||||
SPEED_STEP_REVERSE_037 = 0x26, // 00100110bin 038dec
|
||||
SPEED_STEP_REVERSE_038 = 0x27, // 00100111bin 039dec
|
||||
SPEED_STEP_REVERSE_039 = 0x28, // 00101000bin 040dec
|
||||
SPEED_STEP_REVERSE_040 = 0x29, // 00101001bin 041dec
|
||||
SPEED_STEP_REVERSE_041 = 0x2A, // 00101010bin 042dec
|
||||
SPEED_STEP_REVERSE_042 = 0x2B, // 00101011bin 043dec
|
||||
SPEED_STEP_REVERSE_043 = 0x2C, // 00101100bin 044dec
|
||||
SPEED_STEP_REVERSE_044 = 0x2D, // 00101101bin 045dec
|
||||
SPEED_STEP_REVERSE_045 = 0x2E, // 00101110bin 046dec
|
||||
SPEED_STEP_REVERSE_046 = 0x2F, // 00101111bin 047dec
|
||||
SPEED_STEP_REVERSE_047 = 0x30, // 00110000bin 048dec
|
||||
SPEED_STEP_REVERSE_048 = 0x31, // 00110001bin 049dec
|
||||
SPEED_STEP_REVERSE_049 = 0x32, // 00110010bin 050dec
|
||||
SPEED_STEP_REVERSE_050 = 0x33, // 00110011bin 051dec
|
||||
SPEED_STEP_REVERSE_051 = 0x34, // 00110100bin 052dec
|
||||
SPEED_STEP_REVERSE_052 = 0x35, // 00110101bin 053dec
|
||||
SPEED_STEP_REVERSE_053 = 0x36, // 00110110bin 054dec
|
||||
SPEED_STEP_REVERSE_054 = 0x37, // 00110111bin 055dec
|
||||
SPEED_STEP_REVERSE_055 = 0x38, // 00111000bin 056dec
|
||||
SPEED_STEP_REVERSE_056 = 0x39, // 00111001bin 057dec
|
||||
SPEED_STEP_REVERSE_057 = 0x3A, // 00111010bin 058dec
|
||||
SPEED_STEP_REVERSE_058 = 0x3B, // 00111011bin 059dec
|
||||
SPEED_STEP_REVERSE_059 = 0x3C, // 00111100bin 060dec
|
||||
SPEED_STEP_REVERSE_060 = 0x3D, // 00111101bin 061dec
|
||||
SPEED_STEP_REVERSE_061 = 0x3E, // 00111110bin 062dec
|
||||
SPEED_STEP_REVERSE_062 = 0x3F, // 00111111bin 063dec
|
||||
SPEED_STEP_REVERSE_063 = 0x40, // 01000000bin 064dec
|
||||
SPEED_STEP_REVERSE_064 = 0x41, // 01000001bin 065dec
|
||||
SPEED_STEP_REVERSE_065 = 0x42, // 01000010bin 066dec
|
||||
SPEED_STEP_REVERSE_066 = 0x43, // 01000011bin 067dec
|
||||
SPEED_STEP_REVERSE_067 = 0x44, // 01000100bin 068dec
|
||||
SPEED_STEP_REVERSE_068 = 0x45, // 01000101bin 069dec
|
||||
SPEED_STEP_REVERSE_069 = 0x46, // 01000110bin 070dec
|
||||
SPEED_STEP_REVERSE_070 = 0x47, // 01000111bin 071dec
|
||||
SPEED_STEP_REVERSE_071 = 0x48, // 01001000bin 072dec
|
||||
SPEED_STEP_REVERSE_072 = 0x49, // 01001001bin 073dec
|
||||
SPEED_STEP_REVERSE_073 = 0x4A, // 01001010bin 074dec
|
||||
SPEED_STEP_REVERSE_074 = 0x4B, // 01001011bin 075dec
|
||||
SPEED_STEP_REVERSE_075 = 0x4C, // 01001100bin 076dec
|
||||
SPEED_STEP_REVERSE_076 = 0x4D, // 01001101bin 077dec
|
||||
SPEED_STEP_REVERSE_077 = 0x4E, // 01001110bin 078dec
|
||||
SPEED_STEP_REVERSE_078 = 0x4F, // 01001111bin 079dec
|
||||
SPEED_STEP_REVERSE_079 = 0x50, // 01010000bin 080dec
|
||||
SPEED_STEP_REVERSE_080 = 0x51, // 01010001bin 081dec
|
||||
SPEED_STEP_REVERSE_081 = 0x52, // 01010010bin 082dec
|
||||
SPEED_STEP_REVERSE_082 = 0x53, // 01010011bin 083dec
|
||||
SPEED_STEP_REVERSE_083 = 0x54, // 01010100bin 084dec
|
||||
SPEED_STEP_REVERSE_084 = 0x55, // 01010101bin 085dec
|
||||
SPEED_STEP_REVERSE_085 = 0x56, // 01010110bin 086dec
|
||||
SPEED_STEP_REVERSE_086 = 0x57, // 01010111bin 087dec
|
||||
SPEED_STEP_REVERSE_087 = 0x58, // 01011000bin 088dec
|
||||
SPEED_STEP_REVERSE_088 = 0x59, // 01011001bin 089dec
|
||||
SPEED_STEP_REVERSE_089 = 0x5A, // 01011010bin 090dec
|
||||
SPEED_STEP_REVERSE_090 = 0x5B, // 01011011bin 091dec
|
||||
SPEED_STEP_REVERSE_091 = 0x5C, // 01011100bin 092dec
|
||||
SPEED_STEP_REVERSE_092 = 0x5D, // 01011101bin 093dec
|
||||
SPEED_STEP_REVERSE_093 = 0x5E, // 01011110bin 094dec
|
||||
SPEED_STEP_REVERSE_094 = 0x5F, // 01011111bin 095dec
|
||||
SPEED_STEP_REVERSE_095 = 0x60, // 01100000bin 096dec
|
||||
SPEED_STEP_REVERSE_096 = 0x61, // 01100001bin 097dec
|
||||
SPEED_STEP_REVERSE_097 = 0x62, // 01100010bin 098dec
|
||||
SPEED_STEP_REVERSE_098 = 0x63, // 01100011bin 099dec
|
||||
SPEED_STEP_REVERSE_099 = 0x64, // 01100100bin 100dec
|
||||
SPEED_STEP_REVERSE_100 = 0x65, // 01100101bin 101dec
|
||||
SPEED_STEP_REVERSE_101 = 0x66, // 01100110bin 102dec
|
||||
SPEED_STEP_REVERSE_102 = 0x67, // 01100111bin 103dec
|
||||
SPEED_STEP_REVERSE_103 = 0x68, // 01101000bin 104dec
|
||||
SPEED_STEP_REVERSE_104 = 0x69, // 01101001bin 105dec
|
||||
SPEED_STEP_REVERSE_105 = 0x6A, // 01101010bin 106dec
|
||||
SPEED_STEP_REVERSE_106 = 0x6B, // 01101011bin 107dec
|
||||
SPEED_STEP_REVERSE_107 = 0x6C, // 01101100bin 108dec
|
||||
SPEED_STEP_REVERSE_108 = 0x6D, // 01101101bin 109dec
|
||||
SPEED_STEP_REVERSE_109 = 0x6E, // 01101110bin 110dec
|
||||
SPEED_STEP_REVERSE_110 = 0x6F, // 01101111bin 111dec
|
||||
SPEED_STEP_REVERSE_111 = 0x70, // 01110000bin 112dec
|
||||
SPEED_STEP_REVERSE_112 = 0x71, // 01110001bin 113dec
|
||||
SPEED_STEP_REVERSE_113 = 0x72, // 01110010bin 114dec
|
||||
SPEED_STEP_REVERSE_114 = 0x73, // 01110011bin 115dec
|
||||
SPEED_STEP_REVERSE_115 = 0x74, // 01110100bin 116dec
|
||||
SPEED_STEP_REVERSE_116 = 0x75, // 01110101bin 117dec
|
||||
SPEED_STEP_REVERSE_117 = 0x76, // 01110110bin 118dec
|
||||
SPEED_STEP_REVERSE_118 = 0x77, // 01110111bin 119dec
|
||||
SPEED_STEP_REVERSE_119 = 0x78, // 01111000bin 120dec
|
||||
SPEED_STEP_REVERSE_120 = 0x79, // 01111001bin 121dec
|
||||
SPEED_STEP_REVERSE_121 = 0x7A, // 01111010bin 122dec
|
||||
SPEED_STEP_REVERSE_122 = 0x7B, // 01111011bin 123dec
|
||||
SPEED_STEP_REVERSE_123 = 0x7C, // 01111100bin 124dec
|
||||
SPEED_STEP_REVERSE_124 = 0x7D, // 01111101bin 125dec
|
||||
SPEED_STEP_REVERSE_125 = 0x7E, // 01111110bin 126dec
|
||||
SPEED_STEP_REVERSE_126 = 0x7F, // 01111111bin 127dec
|
||||
|
||||
#define LOG(level, ...) { \
|
||||
if (level <= LOGLEVEL) { \
|
||||
printf("[core%u] ", get_core_num()); \
|
||||
printf(__VA_ARGS__); \
|
||||
} \
|
||||
}
|
||||
|
||||
// Constant Value of 125 x 10^6
|
||||
#define _125M 125000000
|
||||
SPEED_STEP_FORWARD_STOP = 0x80, // 10000000bin 128dec
|
||||
SPEED_STEP_FORWARD_EMERGENCY_STOP = 0x81, // 10000001bin 129dec
|
||||
SPEED_STEP_FORWARD_001 = 0x82, // 10000010bin 130dec
|
||||
SPEED_STEP_FORWARD_002 = 0x83, // 10000011bin 131dec
|
||||
SPEED_STEP_FORWARD_003 = 0x84, // 10000100bin 132dec
|
||||
SPEED_STEP_FORWARD_004 = 0x85, // 10000101bin 133dec
|
||||
SPEED_STEP_FORWARD_005 = 0x86, // 10000110bin 134dec
|
||||
SPEED_STEP_FORWARD_006 = 0x87, // 10000111bin 135dec
|
||||
SPEED_STEP_FORWARD_007 = 0x88, // 10001000bin 136dec
|
||||
SPEED_STEP_FORWARD_008 = 0x89, // 10001001bin 137dec
|
||||
SPEED_STEP_FORWARD_009 = 0x8A, // 10001010bin 138dec
|
||||
SPEED_STEP_FORWARD_010 = 0x8B, // 10001011bin 139dec
|
||||
SPEED_STEP_FORWARD_011 = 0x8C, // 10001100bin 140dec
|
||||
SPEED_STEP_FORWARD_012 = 0x8D, // 10001101bin 141dec
|
||||
SPEED_STEP_FORWARD_013 = 0x8E, // 10001110bin 142dec
|
||||
SPEED_STEP_FORWARD_014 = 0x8F, // 10001111bin 143dec
|
||||
SPEED_STEP_FORWARD_015 = 0x90, // 10010000bin 144dec
|
||||
SPEED_STEP_FORWARD_016 = 0x91, // 10010001bin 145dec
|
||||
SPEED_STEP_FORWARD_017 = 0x92, // 10010010bin 146dec
|
||||
SPEED_STEP_FORWARD_018 = 0x93, // 10010011bin 147dec
|
||||
SPEED_STEP_FORWARD_019 = 0x94, // 10010100bin 148dec
|
||||
SPEED_STEP_FORWARD_020 = 0x95, // 10010101bin 149dec
|
||||
SPEED_STEP_FORWARD_021 = 0x96, // 10010110bin 150dec
|
||||
SPEED_STEP_FORWARD_022 = 0x97, // 10010111bin 151dec
|
||||
SPEED_STEP_FORWARD_023 = 0x98, // 10011000bin 152dec
|
||||
SPEED_STEP_FORWARD_024 = 0x99, // 10011001bin 153dec
|
||||
SPEED_STEP_FORWARD_025 = 0x9A, // 10011010bin 154dec
|
||||
SPEED_STEP_FORWARD_026 = 0x9B, // 10011011bin 155dec
|
||||
SPEED_STEP_FORWARD_027 = 0x9C, // 10011100bin 156dec
|
||||
SPEED_STEP_FORWARD_028 = 0x9D, // 10011101bin 157dec
|
||||
SPEED_STEP_FORWARD_029 = 0x9E, // 10011110bin 158dec
|
||||
SPEED_STEP_FORWARD_030 = 0x9F, // 10011111bin 159dec
|
||||
SPEED_STEP_FORWARD_031 = 0xA0, // 10100000bin 160dec
|
||||
SPEED_STEP_FORWARD_032 = 0xA1, // 10100001bin 161dec
|
||||
SPEED_STEP_FORWARD_033 = 0xA2, // 10100010bin 162dec
|
||||
SPEED_STEP_FORWARD_034 = 0xA3, // 10100011bin 163dec
|
||||
SPEED_STEP_FORWARD_035 = 0xA4, // 10100100bin 164dec
|
||||
SPEED_STEP_FORWARD_036 = 0xA5, // 10100101bin 165dec
|
||||
SPEED_STEP_FORWARD_037 = 0xA6, // 10100110bin 166dec
|
||||
SPEED_STEP_FORWARD_038 = 0xA7, // 10100111bin 167dec
|
||||
SPEED_STEP_FORWARD_039 = 0xA8, // 10101000bin 168dec
|
||||
SPEED_STEP_FORWARD_040 = 0xA9, // 10101001bin 169dec
|
||||
SPEED_STEP_FORWARD_041 = 0xAA, // 10101010bin 170dec
|
||||
SPEED_STEP_FORWARD_042 = 0xAB, // 10101011bin 171dec
|
||||
SPEED_STEP_FORWARD_043 = 0xAC, // 10101100bin 172dec
|
||||
SPEED_STEP_FORWARD_044 = 0xAD, // 10101101bin 173dec
|
||||
SPEED_STEP_FORWARD_045 = 0xAE, // 10101110bin 174dec
|
||||
SPEED_STEP_FORWARD_046 = 0xAF, // 10101111bin 175dec
|
||||
SPEED_STEP_FORWARD_047 = 0xB0, // 10110000bin 176dec
|
||||
SPEED_STEP_FORWARD_048 = 0xB1, // 10110001bin 177dec
|
||||
SPEED_STEP_FORWARD_049 = 0xB2, // 10110010bin 178dec
|
||||
SPEED_STEP_FORWARD_050 = 0xB3, // 10110011bin 179dec
|
||||
SPEED_STEP_FORWARD_051 = 0xB4, // 10110100bin 180dec
|
||||
SPEED_STEP_FORWARD_052 = 0xB5, // 10110101bin 181dec
|
||||
SPEED_STEP_FORWARD_053 = 0xB6, // 10110110bin 182dec
|
||||
SPEED_STEP_FORWARD_054 = 0xB7, // 10110111bin 183dec
|
||||
SPEED_STEP_FORWARD_055 = 0xB8, // 10111000bin 184dec
|
||||
SPEED_STEP_FORWARD_056 = 0xB9, // 10111001bin 185dec
|
||||
SPEED_STEP_FORWARD_057 = 0xBA, // 10111010bin 186dec
|
||||
SPEED_STEP_FORWARD_058 = 0xBB, // 10111011bin 187dec
|
||||
SPEED_STEP_FORWARD_059 = 0xBC, // 10111100bin 188dec
|
||||
SPEED_STEP_FORWARD_060 = 0xBD, // 10111101bin 189dec
|
||||
SPEED_STEP_FORWARD_061 = 0xBE, // 10111110bin 190dec
|
||||
SPEED_STEP_FORWARD_062 = 0xBF, // 10111111bin 191dec
|
||||
SPEED_STEP_FORWARD_063 = 0xC0, // 11000000bin 192dec
|
||||
SPEED_STEP_FORWARD_064 = 0xC1, // 11000001bin 193dec
|
||||
SPEED_STEP_FORWARD_065 = 0xC2, // 11000010bin 194dec
|
||||
SPEED_STEP_FORWARD_066 = 0xC3, // 11000011bin 195dec
|
||||
SPEED_STEP_FORWARD_067 = 0xC4, // 11000100bin 196dec
|
||||
SPEED_STEP_FORWARD_068 = 0xC5, // 11000101bin 197dec
|
||||
SPEED_STEP_FORWARD_069 = 0xC6, // 11000110bin 198dec
|
||||
SPEED_STEP_FORWARD_070 = 0xC7, // 11000111bin 199dec
|
||||
SPEED_STEP_FORWARD_071 = 0xC8, // 11001000bin 200dec
|
||||
SPEED_STEP_FORWARD_072 = 0xC9, // 11001001bin 201dec
|
||||
SPEED_STEP_FORWARD_073 = 0xCA, // 11001010bin 202dec
|
||||
SPEED_STEP_FORWARD_074 = 0xCB, // 11001011bin 203dec
|
||||
SPEED_STEP_FORWARD_075 = 0xCC, // 11001100bin 204dec
|
||||
SPEED_STEP_FORWARD_076 = 0xCD, // 11001101bin 205dec
|
||||
SPEED_STEP_FORWARD_077 = 0xCE, // 11001110bin 206dec
|
||||
SPEED_STEP_FORWARD_078 = 0xCF, // 11001111bin 207dec
|
||||
SPEED_STEP_FORWARD_079 = 0xD0, // 11010000bin 208dec
|
||||
SPEED_STEP_FORWARD_080 = 0xD1, // 11010001bin 209dec
|
||||
SPEED_STEP_FORWARD_081 = 0xD2, // 11010010bin 210dec
|
||||
SPEED_STEP_FORWARD_082 = 0xD3, // 11010011bin 211dec
|
||||
SPEED_STEP_FORWARD_083 = 0xD4, // 11010100bin 212dec
|
||||
SPEED_STEP_FORWARD_084 = 0xD5, // 11010101bin 213dec
|
||||
SPEED_STEP_FORWARD_085 = 0xD6, // 11010110bin 214dec
|
||||
SPEED_STEP_FORWARD_086 = 0xD7, // 11010111bin 215dec
|
||||
SPEED_STEP_FORWARD_087 = 0xD8, // 11011000bin 216dec
|
||||
SPEED_STEP_FORWARD_088 = 0xD9, // 11011001bin 217dec
|
||||
SPEED_STEP_FORWARD_089 = 0xDA, // 11011010bin 218dec
|
||||
SPEED_STEP_FORWARD_090 = 0xDB, // 11011011bin 219dec
|
||||
SPEED_STEP_FORWARD_091 = 0xDC, // 11011100bin 220dec
|
||||
SPEED_STEP_FORWARD_092 = 0xDD, // 11011101bin 221dec
|
||||
SPEED_STEP_FORWARD_093 = 0xDE, // 11011110bin 222dec
|
||||
SPEED_STEP_FORWARD_094 = 0xDF, // 11011111bin 223dec
|
||||
SPEED_STEP_FORWARD_095 = 0xE0, // 11100000bin 224dec
|
||||
SPEED_STEP_FORWARD_096 = 0xE1, // 11100001bin 225dec
|
||||
SPEED_STEP_FORWARD_097 = 0xE2, // 11100010bin 226dec
|
||||
SPEED_STEP_FORWARD_098 = 0xE3, // 11100011bin 227dec
|
||||
SPEED_STEP_FORWARD_099 = 0xE4, // 11100100bin 228dec
|
||||
SPEED_STEP_FORWARD_100 = 0xE5, // 11100101bin 229dec
|
||||
SPEED_STEP_FORWARD_101 = 0xE6, // 11100110bin 230dec
|
||||
SPEED_STEP_FORWARD_102 = 0xE7, // 11100111bin 231dec
|
||||
SPEED_STEP_FORWARD_103 = 0xE8, // 11101000bin 232dec
|
||||
SPEED_STEP_FORWARD_104 = 0xE9, // 11101001bin 233dec
|
||||
SPEED_STEP_FORWARD_105 = 0xEA, // 11101010bin 234dec
|
||||
SPEED_STEP_FORWARD_106 = 0xEB, // 11101011bin 235dec
|
||||
SPEED_STEP_FORWARD_107 = 0xEC, // 11101100bin 236dec
|
||||
SPEED_STEP_FORWARD_108 = 0xED, // 11101101bin 237dec
|
||||
SPEED_STEP_FORWARD_109 = 0xEE, // 11101110bin 238dec
|
||||
SPEED_STEP_FORWARD_110 = 0xEF, // 11101111bin 239dec
|
||||
SPEED_STEP_FORWARD_111 = 0xF0, // 11110000bin 240dec
|
||||
SPEED_STEP_FORWARD_112 = 0xF1, // 11110001bin 241dec
|
||||
SPEED_STEP_FORWARD_113 = 0xF2, // 11110010bin 242dec
|
||||
SPEED_STEP_FORWARD_114 = 0xF3, // 11110011bin 243dec
|
||||
SPEED_STEP_FORWARD_115 = 0xF4, // 11110100bin 244dec
|
||||
SPEED_STEP_FORWARD_116 = 0xF5, // 11110101bin 245dec
|
||||
SPEED_STEP_FORWARD_117 = 0xF6, // 11110110bin 246dec
|
||||
SPEED_STEP_FORWARD_118 = 0xF7, // 11110111bin 247dec
|
||||
SPEED_STEP_FORWARD_119 = 0xF8, // 11111000bin 248dec
|
||||
SPEED_STEP_FORWARD_120 = 0xF9, // 11111001bin 249dec
|
||||
SPEED_STEP_FORWARD_121 = 0xFA, // 11111010bin 250dec
|
||||
SPEED_STEP_FORWARD_122 = 0xFB, // 11111011bin 251dec
|
||||
SPEED_STEP_FORWARD_123 = 0xFC, // 11111100bin 252dec
|
||||
SPEED_STEP_FORWARD_124 = 0xFD, // 11111101bin 253dec
|
||||
SPEED_STEP_FORWARD_125 = 0xFE, // 11111110bin 254dec
|
||||
SPEED_STEP_FORWARD_126 = 0xFF, // 11111111bin 255dec
|
||||
} speed_step_t;
|
||||
|
||||
|
||||
extern repeating_timer_t pid_control_timer;
|
||||
@@ -53,7 +314,6 @@ extern uint8_t speed_step_target_prev;
|
||||
|
||||
|
||||
void core1_entry();
|
||||
void write_cv_handler(uint16_t cv_index, uint8_t cv_data);
|
||||
uint32_t get_32bit_CV (uint16_t CV_start_index);
|
||||
uint16_t get_16bit_CV (uint16_t CV_start_index);
|
||||
/*!
|
||||
@@ -93,4 +353,6 @@ void set_error(error_t err);
|
||||
|
||||
void clear_error(error_t err);
|
||||
|
||||
uint32_t get_error_state();
|
||||
void clear_all_errors();
|
||||
|
||||
error_t get_error_state();
|
||||
@@ -0,0 +1,48 @@
|
||||
|
||||
def print_speed_steps():
|
||||
"""
|
||||
This script generates and prints speed step definitions for both reverse and forward directions.
|
||||
Functions:
|
||||
print_speed_steps(): Generates and prints speed step definitions in hexadecimal and binary formats used in shared.h.
|
||||
- Reverse speed steps range from 0x00 to 0x7F.
|
||||
- Forward speed steps range from 0x80 to 0xFF.
|
||||
|
||||
* in NMRA Standard 9.2.1 section 2.3.2.1 or RCN-212 section 2.2.2.
|
||||
*
|
||||
* The following speed steps are defined:
|
||||
*
|
||||
* | DEC | BIN | DIRECTION | DEFINITION |
|
||||
* |-----|-----------|-----------|----------------------|
|
||||
* | 000 | 0000 0000 | Reverse | STOP |
|
||||
* | 001 | 0000 0001 | Reverse | EMERGENCY STOP |
|
||||
* | 002 | 0000 0010 | Reverse | Speed step 001 |
|
||||
* | 003 | 0000 0011 | Reverse | Speed step 002 |
|
||||
* | 004 | 0000 0100 | Reverse | Speed step 003 |
|
||||
* | ... | .... .... | Reverse | ... |
|
||||
* | 126 | 0111 1110 | Reverse | Speed step 125 |
|
||||
* | 127 | 0111 1111 | Reverse | Speed step 126 |
|
||||
* | 128 | 1000 0000 | Forward | STOP |
|
||||
* | 129 | 1000 0001 | Forward | EMERGENCY STOP |
|
||||
* | 130 | 1000 0010 | Forward | Speed step 001 |
|
||||
* | 131 | 1000 0011 | Forward | Speed step 002 |
|
||||
* | 132 | 1000 0100 | Forward | Speed step 003 |
|
||||
* | ... | .... .... | Forward | ... |
|
||||
* | 254 | 1111 1110 | Forward | Speed step 125 |
|
||||
* | 255 | 1111 1111 | Forward | Speed step 126 |
|
||||
"""
|
||||
# Reverse speed steps
|
||||
print(f"SPEED_STEP_REVERSE_STOP = 0x{0:02X}, // {0:08b}bin {0:03d}dec")
|
||||
print(f"SPEED_STEP_REVERSE_EMERGENCY_STOP = 0x{1:02X}, // {1:08b}bin {1:03d}dec")
|
||||
for i in range(1, 127):
|
||||
print(f"SPEED_STEP_REVERSE_{i:03d} = 0x{i+1:02X}, // {i+1:08b}bin {i+1:03d}dec")
|
||||
|
||||
print("\n\n")
|
||||
|
||||
# Forward speed steps
|
||||
print(f"SPEED_STEP_FORWARD_STOP = 0x{128:02X}, // {128:08b}bin {128:03d}dec")
|
||||
print(f"SPEED_STEP_FORWARD_EMERGENCY_STOP = 0x{129:02X}, // {129:08b}bin {129:03d}dec")
|
||||
for i in range(1, 127):
|
||||
print(f"SPEED_STEP_FORWARD_{i:03d} = 0x{i+129:02X}, // {i+129:08b}bin {i+129:03d}dec")
|
||||
|
||||
# Call the function
|
||||
print_speed_steps()
|
||||
Reference in new issue
Block a user