Moved all software related files to Software folder; Now using flash safe execution functions of pico_flash (pico SDK) lib; Added error handling; Added RP2040-Decoder-board.h definition; Adapted CMakeLists.txt for use with vscode pi pico extension;
This commit is contained in:
1 parent
e8a8ac6d98
commit
7b1d5ed152
12 files changed
+1452
-1271
No files matched your search
@@ -0,0 +1 @@
|
||||
build
|
||||
@@ -0,0 +1,75 @@
|
||||
# Generated Cmake Pico project file
|
||||
|
||||
cmake_minimum_required(VERSION 3.13)
|
||||
|
||||
set(CMAKE_C_STANDARD 11)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
||||
|
||||
# Initialise pico_sdk from installed location
|
||||
# (note this can come from environment, CMake cache etc)
|
||||
|
||||
# == DO NOT EDIT THE FOLLOWING LINES for the Raspberry Pi Pico VS Code Extension to work ==
|
||||
if(WIN32)
|
||||
set(USERHOME $ENV{USERPROFILE})
|
||||
else()
|
||||
set(USERHOME $ENV{HOME})
|
||||
endif()
|
||||
set(sdkVersion 2.1.0)
|
||||
set(toolchainVersion 13_3_Rel1)
|
||||
set(picotoolVersion 2.1.0)
|
||||
set(picoVscode ${USERHOME}/.pico-sdk/cmake/pico-vscode.cmake)
|
||||
if (EXISTS ${picoVscode})
|
||||
include(${picoVscode})
|
||||
endif()
|
||||
# ====================================================================================
|
||||
set(PICO_BOARD_HEADER_DIRS ${CMAKE_SOURCE_DIR})
|
||||
set(PICO_BOARD RP2040-Decoder-board CACHE STRING "Board type")
|
||||
|
||||
# Pull in Raspberry Pi Pico SDK (must be before project)
|
||||
include(pico_sdk_import.cmake)
|
||||
|
||||
project(RP2040-Decoder C CXX ASM)
|
||||
|
||||
# Initialise the Raspberry Pi Pico SDK
|
||||
pico_sdk_init()
|
||||
|
||||
add_compile_options(#-Wall
|
||||
-O0
|
||||
-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)
|
||||
|
||||
# Add executable
|
||||
add_executable( RP2040-Decoder
|
||||
core0.c
|
||||
core0.h
|
||||
core1.c
|
||||
core1.h
|
||||
shared.c
|
||||
shared.h )
|
||||
|
||||
pico_set_program_name(RP2040-Decoder "RP2040-Decoder")
|
||||
pico_set_program_version(RP2040-Decoder "1.0")
|
||||
|
||||
# Modify the below lines to enable/disable output over UART/USB
|
||||
pico_enable_stdio_uart(RP2040-Decoder 0)
|
||||
pico_enable_stdio_usb(RP2040-Decoder 1)
|
||||
|
||||
# Add the standard library to the build
|
||||
target_link_libraries( RP2040-Decoder
|
||||
pico_stdlib
|
||||
hardware_pwm
|
||||
hardware_adc
|
||||
hardware_flash
|
||||
pico_multicore
|
||||
pico_flash)
|
||||
|
||||
# Add the standard include files to the build
|
||||
target_include_directories(RP2040-Decoder PRIVATE
|
||||
${CMAKE_CURRENT_LIST_DIR}
|
||||
)
|
||||
|
||||
# create map/bin/hex file etc.
|
||||
pico_add_extra_outputs(RP2040-Decoder)
|
||||
|
||||
+1135
File diff suppressed because it is too large.
Load diff
@@ -0,0 +1,63 @@
|
||||
/*
|
||||
* Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
*/
|
||||
|
||||
// -----------------------------------------------------
|
||||
// NOTE: THIS HEADER IS ALSO INCLUDED BY ASSEMBLER SO
|
||||
// SHOULD ONLY CONSIST OF PREPROCESSOR DIRECTIVES
|
||||
// -----------------------------------------------------
|
||||
|
||||
// Modified version of "boards/pico.h" for using the RP2040-Decoder board
|
||||
|
||||
// pico_cmake_set PICO_PLATFORM=rp2040
|
||||
|
||||
#ifndef _BOARDS_RP2040_DECODER_H
|
||||
#define _BOARDS_RP2040_DECODER_H
|
||||
|
||||
// For board detection
|
||||
#define RP2040_DECODER
|
||||
|
||||
// In case the xosc takes longer to stabilize than usual
|
||||
#ifndef PICO_XOSC_STARTUP_DELAY_MULTIPLIER
|
||||
#define PICO_XOSC_STARTUP_DELAY_MULTIPLIER 64
|
||||
#endif
|
||||
|
||||
// --- UART ---
|
||||
#ifndef PICO_DEFAULT_UART
|
||||
#define PICO_DEFAULT_UART 0
|
||||
#endif
|
||||
#ifndef PICO_DEFAULT_UART_TX_PIN
|
||||
#define PICO_DEFAULT_UART_TX_PIN 0
|
||||
#endif
|
||||
#ifndef PICO_DEFAULT_UART_RX_PIN
|
||||
#define PICO_DEFAULT_UART_RX_PIN 1
|
||||
#endif
|
||||
|
||||
// --- LED ---
|
||||
#ifndef RP2040_DECODER_DEFAULT_LED_PIN
|
||||
#define RP2040_DECODER_DEFAULT_LED_PIN 12
|
||||
#endif
|
||||
|
||||
// --- FLASH ---
|
||||
|
||||
// Flash size is 8 MiB for W25Q64JVSSIQ if you decide to use another capacity flash IC change the size to the corresponding value
|
||||
#ifndef PICO_FLASH_SIZE_BYTES
|
||||
#define PICO_FLASH_SIZE_BYTES (8 * 1024 * 1024)
|
||||
#endif
|
||||
// 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)
|
||||
|
||||
#ifndef PICO_FLASH_SPI_CLKDIV
|
||||
#define PICO_FLASH_SPI_CLKDIV 2
|
||||
#endif
|
||||
|
||||
|
||||
#ifndef PICO_RP2040_B0_SUPPORTED
|
||||
#define PICO_RP2040_B0_SUPPORTED 1
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,675 @@
|
||||
//////////////////////////
|
||||
// RP2040-Decoder //
|
||||
// Gabriel Koppenstein //
|
||||
// core0.c //
|
||||
//////////////////////////
|
||||
|
||||
#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
|
||||
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;
|
||||
|
||||
|
||||
// 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
|
||||
|
||||
// Read the JEDEC ID
|
||||
flash_do_cmd(txbuf, rxbuf, sizeof(txbuf));
|
||||
|
||||
// The third byte of the response (rxbuf[3]) contains the log2 size (e.g., 0x18 for 16 MB)
|
||||
if (rxbuf[3] == 0) {
|
||||
set_error(FLASH_SIZE_READBACK_FAILURE);
|
||||
}
|
||||
|
||||
// Calculate the actual size (2^size_code bytes)
|
||||
uint32_t flash_size = (1u<<rxbuf[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
|
||||
static void call_flash_range_erase(void *param) {
|
||||
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)
|
||||
// Check byte count (Must be a multiple of 4096 bytes (one sector))
|
||||
if ((offset % FLASH_SECTOR_SIZE != 0) || (byte_count % FLASH_SECTOR_SIZE !=0)) {
|
||||
set_error(FLASH_SAFE_EXECUTE_PROGRAM_FAILURE);
|
||||
return;
|
||||
}
|
||||
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) {
|
||||
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];
|
||||
// Check for offset alignment (Must be aligned to a 256-byte flash page)
|
||||
// Check byte count (Must be a multiple of 256 bytes (one page))
|
||||
if ((offset % FLASH_PAGE_SIZE != 0) || (byte_count % FLASH_PAGE_SIZE != 0)) {
|
||||
set_error(FLASH_SAFE_EXECUTE_PROGRAM_FAILURE);
|
||||
return;
|
||||
}
|
||||
flash_range_program(offset, data, byte_count);
|
||||
}
|
||||
|
||||
|
||||
float two_std_dev(const float arr[], const uint32_t length) {
|
||||
// Calculate arithmetic average
|
||||
float sum = 0;
|
||||
for (uint32_t i = 0; i < length; ++i) {
|
||||
sum += arr[i];
|
||||
}
|
||||
const float x_avg = sum / (float)length;
|
||||
|
||||
// Calculate sample variance and sampled standard deviation
|
||||
sum = 0;
|
||||
for (uint32_t i = 0; i < length; ++i) {
|
||||
sum += (arr[i] - x_avg) * (arr[i] - x_avg);
|
||||
}
|
||||
const float variance = sum / (float)(length-1);
|
||||
const float std_dev = sqrtf(variance);
|
||||
|
||||
// Sum every element that is within two times the sampled standard deviation and compute new average
|
||||
sum = 0;
|
||||
uint32_t counter = 0;
|
||||
for (uint32_t i = 0; i < length; ++i) {
|
||||
const float diff = fabsf(arr[i] - x_avg);
|
||||
if ( diff < std_dev * 2.0f ){
|
||||
sum += arr[i];
|
||||
counter++;
|
||||
}
|
||||
}
|
||||
return sum / (float) 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) {
|
||||
LOG(1, "adc_offset_adjustment");
|
||||
|
||||
float offsets_fwd[n];
|
||||
float offsets_rev[n];
|
||||
// Set PWM level to 0 just to be sure
|
||||
pwm_set_gpio_level(MOTOR_FWD_PIN, 0);
|
||||
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?
|
||||
adc_run(true);
|
||||
for (uint32_t i = 0; i < n; i++) {
|
||||
offsets_rev[i] = (float) adc_fifo_get_blocking();
|
||||
}
|
||||
adc_run(false);
|
||||
adc_fifo_drain();
|
||||
busy_wait_ms(1000);
|
||||
|
||||
// Measure n times in "forward direction" (GPIO 29)
|
||||
adc_select_input(3);
|
||||
adc_run(true);
|
||||
for (uint32_t i = 0; i < n; i++) {
|
||||
offsets_fwd[i] = (float) adc_fifo_get_blocking();
|
||||
}
|
||||
adc_run(false);
|
||||
adc_fifo_drain();
|
||||
|
||||
// Find overall average discarding outliers in measurement
|
||||
const float offset_avg_fwd = two_std_dev(offsets_fwd, n);
|
||||
const float offset_avg_rev = two_std_dev(offsets_rev, n);
|
||||
const float overall_avg = (offset_avg_fwd + offset_avg_rev) / 2;
|
||||
const uint8_t offset = (uint8_t) roundf(overall_avg);
|
||||
|
||||
LOG(1, "new adc offset CV[171] (%f): (uint8_t)%d\n", overall_avg, offset);
|
||||
|
||||
// Create temporary array -> change CV 172 in temp array -> erase flash -> write temp array to flash
|
||||
uint8_t CV_ARRAY_TEMP[CV_ARRAY_SIZE];
|
||||
memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP));
|
||||
CV_ARRAY_TEMP[171] = 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);
|
||||
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);
|
||||
if(ret_val != PICO_OK){
|
||||
set_error(FLASH_SAFE_EXECUTE_PROGRAM_FAILURE);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void acknowledge() {
|
||||
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);
|
||||
pwm_set_gpio_level(MOTOR_FWD_PIN, 0);
|
||||
pwm_set_gpio_level(MOTOR_REV_PIN, max_lvl);
|
||||
busy_wait_ms(3);
|
||||
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) {
|
||||
// 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;
|
||||
if (res) {
|
||||
acknowledge();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void verify_cv_byte(const uint16_t cv_address, const uint8_t 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) {
|
||||
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);
|
||||
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);
|
||||
if(ret_val != PICO_OK){
|
||||
set_error(FLASH_SAFE_EXECUTE_PROGRAM_FAILURE);
|
||||
return;
|
||||
}
|
||||
acknowledge();
|
||||
}
|
||||
|
||||
|
||||
// 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) {
|
||||
switch (cv_index) {
|
||||
case 0: //CV_1
|
||||
// CV_1 => Value = 0 is not allowed
|
||||
if (cv_data == 0 || cv_data > 127) {
|
||||
break;
|
||||
}
|
||||
else {
|
||||
write_cv_byte(cv_index, cv_data);
|
||||
}
|
||||
break;
|
||||
case 6: //CV_7
|
||||
// Read only (CV_7 - Version no.)
|
||||
// ADC offset Adjustment (CV_7; Value = 7);
|
||||
if (cv_data == 7) {
|
||||
LOG(1, "trigger adc offset adjustment via cv7 => 7\n");
|
||||
adc_offset_adjustment(8192);
|
||||
}
|
||||
break;
|
||||
case 7: //CV_8
|
||||
// Read only (CV_8 - Manufacturer ID)
|
||||
// Reset all CVs to Default (CV_8; Value = 8)
|
||||
if (cv_data == 8) {
|
||||
LOG(1, "reset of flash triggered via cv8 => 8\n");
|
||||
uintptr_t params_erase[] = {FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE};
|
||||
int ret_val = flash_safe_execute(call_flash_range_erase, params_erase, UINT32_MAX);
|
||||
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);
|
||||
if(ret_val != PICO_OK){
|
||||
set_error(FLASH_SAFE_EXECUTE_PROGRAM_FAILURE);
|
||||
return;
|
||||
}
|
||||
}
|
||||
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
|
||||
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
|
||||
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
|
||||
break;
|
||||
// Regular CV write is carried out
|
||||
default:
|
||||
write_cv_byte(cv_index, cv_data);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void program_mode(const uint8_t 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;
|
||||
const uint8_t instruction_type = instruction_type_mask & byte_array[number_of_bytes - 1];
|
||||
const uint8_t cv_address_ms_bits_mask = 0b00000011;
|
||||
const uint16_t cv_address_ms_bits = cv_address_ms_bits_mask & byte_array[number_of_bytes - 1];
|
||||
const uint16_t cv_address = byte_array[number_of_bytes - 2] + (cv_address_ms_bits << 8);
|
||||
|
||||
if (instruction_type == 0b000001000) {
|
||||
// Verify CV bit instruction
|
||||
const uint8_t bit_pos_mask = 0b00000111;
|
||||
const uint8_t bit_pos = bit_pos_mask & byte_array[number_of_bytes - 3];
|
||||
const uint8_t bit_val_mask = 0b00000001;
|
||||
const uint8_t bit_val_uint = (byte_array[number_of_bytes - 3] >> 3) & bit_val_mask;
|
||||
const bool bit_val = bit_val_uint;
|
||||
verify_cv_bit(cv_address, bit_val, bit_pos);
|
||||
}
|
||||
|
||||
else if (instruction_type == 0b000000100) {
|
||||
// Verify CV byte instruction
|
||||
const uint8_t cv_data = byte_array[number_of_bytes - 3];
|
||||
verify_cv_byte(cv_address, cv_data);
|
||||
}
|
||||
|
||||
else if (instruction_type == 0b000001100) {
|
||||
// Write CV byte instruction
|
||||
const uint8_t cv_data = byte_array[number_of_bytes - 3];
|
||||
write_cv_handler(cv_address, cv_data);
|
||||
}
|
||||
busy_wait_ms(5);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void set_outputs(const uint32_t 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;
|
||||
|
||||
// Get enabled output configuration corresponding to set functions and direction
|
||||
uint32_t outputs_to_set = 0;
|
||||
uint32_t temp_mask = 1;
|
||||
for (uint8_t i = 0; i < 32; i++) {
|
||||
if (temp_mask & functions_to_set_bitmask) {
|
||||
const bool direction = get_direction_of_speed_step(speed_step_target);
|
||||
outputs_to_set |= get_32bit_CV(i * 8 + 260 - 4 * direction);
|
||||
}
|
||||
temp_mask <<= 1;
|
||||
}
|
||||
outputs_to_set_PWM &= outputs_to_set; // Outputs with PWM to be enabled
|
||||
outputs_to_set &= ~outputs_to_set_PWM; // Outputs without PWM to be enabled
|
||||
|
||||
// Set regular outputs without PWM, preventing illegal GPIO by masking with GPIO_ALLOWED_OUTPUTS
|
||||
gpio_put_masked(GPIO_ALLOWED_OUTPUTS, outputs_to_set);
|
||||
|
||||
// Set PWM enabled outputs to desired level
|
||||
temp_mask = 1;
|
||||
for (uint8_t i = 0; i < 32; ++i) {
|
||||
if (temp_mask & outputs_to_set_PWM) {
|
||||
pwm_set_gpio_level(i, level_table[i]);
|
||||
pwm_gpio_to_channel(i);
|
||||
pwm_gpio_to_slice_num(i);
|
||||
}
|
||||
else if (temp_mask & PWM_enabled_outputs) {
|
||||
pwm_set_gpio_level(i, 0);
|
||||
}
|
||||
temp_mask <<= 1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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 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.
|
||||
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
|
||||
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
|
||||
if (changes) {
|
||||
set_outputs(active_functions);
|
||||
}
|
||||
prev_active_func = active_functions;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Bitwise XOR for all Bytes -> when result is: "0000 0000" error check is passed.
|
||||
bool error_detection(const int8_t number_of_bytes, const uint8_t *const byte_array) {
|
||||
uint8_t xor_byte = 0;
|
||||
for (int i = 0; i < number_of_bytes; i++) {
|
||||
xor_byte = xor_byte ^ byte_array[i];
|
||||
}
|
||||
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) {
|
||||
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) {
|
||||
// Check for idle message
|
||||
if (byte_array[number_of_bytes - 1] == 255) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Long/extended address decoder configuration (CV_29 Bit5)
|
||||
const bool CV_29_bit5 = (CV_ARRAY_FLASH[28] & 0b00100000) >> 5;
|
||||
uint16_t read_address;
|
||||
// Check if decoder is configured to receive long/extended address (CV_29 Bit5 set)
|
||||
if (CV_29_bit5) {
|
||||
// Check DCC message for long address format
|
||||
if (is_long_address(number_of_bytes, byte_array)) {
|
||||
// DCC message received has long address format
|
||||
// start of transmission -> address_byte_1 -> address_byte_0 -> ... -> end of transmission
|
||||
const uint16_t address_byte_1 = (byte_array[number_of_bytes - 1]) - 192; // Remove long address identifier bits
|
||||
const uint16_t address_byte_0 = (byte_array[number_of_bytes - 2]);
|
||||
read_address = (address_byte_1 << 8) + address_byte_0;
|
||||
const uint16_t long_address = get_16bit_CV(16) & 0x3FFF; // &Mask to remove long address identifier bits
|
||||
return (long_address == read_address);
|
||||
}
|
||||
else {
|
||||
// DCC message received does not have long address format
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Else decoder is configured to receive short address (CV_29 Bit5 not set)
|
||||
// Short address message:
|
||||
// start of transmission -> address_byte_0 -> ... -> end of transmission
|
||||
read_address = byte_array[number_of_bytes - 1];
|
||||
return CV_ARRAY_FLASH[0] == read_address;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Evaluate the type of instruction
|
||||
void instruction_evaluation(const uint8_t number_of_bytes, const uint8_t *const byte_array) {
|
||||
// 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;
|
||||
if (is_long_address(number_of_bytes, byte_array)) {
|
||||
command_byte_start_index = number_of_bytes - 3;
|
||||
}
|
||||
else {
|
||||
command_byte_start_index = number_of_bytes - 2;
|
||||
}
|
||||
const uint8_t command_byte_n = byte_array[command_byte_start_index];
|
||||
|
||||
if (command_byte_n == 0b00111111) {
|
||||
// 0011-1111 (128 Speed Step Control) - 2 Byte length
|
||||
speed_step_target_prev = speed_step_target;
|
||||
speed_step_target = byte_array[command_byte_start_index - 1];
|
||||
// Check for direction change
|
||||
const bool direction_changed = get_direction_of_speed_step(speed_step_target) !=
|
||||
get_direction_of_speed_step(speed_step_target_prev);
|
||||
// In case of a direction change, functions need to be updated because functions depend on direction
|
||||
if (direction_changed) {
|
||||
update_active_functions(0, 0, true);
|
||||
}
|
||||
}
|
||||
|
||||
else if (command_byte_n >> 6 == 0b00000010) {
|
||||
// 10XX-XXXX (Function Group Instruction)
|
||||
switch (command_byte_n >> 4) {
|
||||
case 0b00001011: // F5-F8
|
||||
update_active_functions((command_byte_n << 5), 1, false);
|
||||
break;
|
||||
case 0b00001010: // F9-F12
|
||||
update_active_functions((command_byte_n << 9), 2, false);
|
||||
break;
|
||||
default: // F0-F4
|
||||
update_active_functions((((command_byte_n & 0b00001111) << 1) + ((command_byte_n & 0b00010000) >> 4)), 0, false);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
else if (command_byte_n >> 5 == 0b00000110) {
|
||||
//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);
|
||||
break;
|
||||
case 0b11011111: // F21-F28
|
||||
update_active_functions((byte_array[command_byte_start_index - 1] << 21), 4, false);
|
||||
break;
|
||||
case 0b11011000: // F29-F31
|
||||
update_active_functions((byte_array[command_byte_start_index - 1] << 29), 5, false);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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) {
|
||||
if (byte_array[number_of_bytes - 1] == 0b00000000 && byte_array[number_of_bytes - 2] == 0b00000000) {
|
||||
const bool direction = 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;
|
||||
// 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"
|
||||
speed_step_target = 1 + 128*direction;
|
||||
update_active_functions(0,0,0); // Disables all functions
|
||||
return true;
|
||||
}
|
||||
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;
|
||||
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;
|
||||
if (masked_message_4_bytes == MESSAGE_4_BYTES) number_of_bytes = 4;
|
||||
const uint64_t masked_message_5_bytes = input_bit_buffer & MESSAGE_MASK_5_BYTES;
|
||||
if (masked_message_5_bytes == MESSAGE_5_BYTES) number_of_bytes = 5;
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Sequence of bits gets evaluated
|
||||
void evaluate_message() {
|
||||
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 8Bit array
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
|
||||
// DCC-Signal (GPIO 21) falling edge interrupt
|
||||
void track_signal_fall( const unsigned int gpio, const long unsigned int events) {
|
||||
// Save current timer value
|
||||
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);
|
||||
// When logical high was longer than 87us write 0 bit into buffer otherwise 1
|
||||
bool bit;
|
||||
if (time_logical_high > 87) bit = 0;
|
||||
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);
|
||||
}
|
||||
|
||||
void init_outputs() {
|
||||
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) {
|
||||
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"
|
||||
const uint16_t wrap = get_16bit_CV(115 + 7 * slice);
|
||||
const uint16_t level = get_16bit_CV(118 + 7 * slice + 2 * channel);
|
||||
const uint8_t clock_divider = CV_ARRAY_FLASH[117 + 7 * slice] + 1;
|
||||
gpio_set_function(i, GPIO_FUNC_PWM);
|
||||
pwm_set_wrap(slice, wrap);
|
||||
pwm_set_gpio_level(i, 0);
|
||||
level_table[i] = level;
|
||||
pwm_set_clkdiv_int_frac(slice, clock_divider, 0);
|
||||
pwm_set_enabled(slice, true);
|
||||
}
|
||||
mask <<= 1;
|
||||
}
|
||||
}
|
||||
|
||||
void cv_setup_check() {
|
||||
// Check for flash factory setting and set CV_FLASH_ARRAY to default values when factory condition ("0xFF") is found.
|
||||
if (true || CV_ARRAY_FLASH[64] == 0xFF) {
|
||||
LOG(1, "found cv[64] equals to 0xff, reseting CVs (and CV[8] = 8)\n");
|
||||
const uint8_t arr[4] = {125, 8, 7, 124};
|
||||
program_mode(4, arr); //reset to CV_ARRAY_DEFAULT (write CV_8 = 8)
|
||||
}
|
||||
|
||||
// Check for existing ADC offset setup TODO: always measure?
|
||||
if (false && CV_ARRAY_FLASH[171] == 0xFF) {
|
||||
// Write a value of 7 to read-only CV_7 in order to trigger a ADC offset measurement
|
||||
LOG(1, "found cv[171] equals to 0xff, adc offset adjstments (and cr[7] = 7)\n");
|
||||
const uint8_t arr[4] = {125, 7, 6, 124};
|
||||
program_mode(4, arr); // ADC offset adjustment (write CV_7 = 7)
|
||||
}
|
||||
LOG(1, "int_lim_max %d\n", CV_ARRAY_FLASH[51]);
|
||||
LOG(1, "int_lim_min %d\n", CV_ARRAY_FLASH[52]);
|
||||
}
|
||||
|
||||
|
||||
// Motor PWM initialization
|
||||
void init_motor_pwm(const uint8_t gpio) {
|
||||
// Set GPIO pin to PWM functionality
|
||||
gpio_set_function(gpio, GPIO_FUNC_PWM);
|
||||
// Set wrap counter value saved in CV
|
||||
const uint16_t wrap_counter = (_125M / (CV_ARRAY_FLASH[8] * 100 + 10000)) - 1;
|
||||
const uint32_t slice_num = pwm_gpio_to_slice_num(gpio);
|
||||
pwm_set_clkdiv_int_frac(slice_num, CV_ARRAY_FLASH[173], 0);
|
||||
pwm_set_wrap(slice_num, wrap_counter);
|
||||
// Set initial level to 0
|
||||
pwm_set_gpio_level(gpio, 0);
|
||||
// Enable PWM
|
||||
pwm_set_enabled(slice_num, true);
|
||||
LOG(2, "Initialized Motor PWM on pin: %d; wrap_counter %d; clkdiv %d;\n", gpio, wrap_counter, CV_ARRAY_FLASH[173]);
|
||||
}
|
||||
|
||||
void init_adc() {
|
||||
// Initialize ADC
|
||||
adc_init();
|
||||
// Set ADC GPIO pins to ADC functionality
|
||||
adc_gpio_init(FWD_V_EMF_ADC_PIN);
|
||||
adc_gpio_init(REV_V_EMF_ADC_PIN);
|
||||
// Configure ADC FIFO
|
||||
adc_fifo_setup(true, false, 0, false, false);
|
||||
}
|
||||
|
||||
int main() {
|
||||
if(stdio_init_all() != true){
|
||||
set_error(STDIO_INIT_FAILURE);
|
||||
}
|
||||
multicore_launch_core1(core1_entry);
|
||||
LOG(1, "core0 Initialization...\n");
|
||||
// Wait 1ms to give core1 some time to call flash_safe_execute_core_init()
|
||||
busy_wait_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 CV array for factory state of flash or missing ADC offset setup
|
||||
LOG(1, "Check CV array\n");
|
||||
cv_setup_check();
|
||||
LOG(1, "Initializing Motor PWM...\n");
|
||||
init_motor_pwm(MOTOR_FWD_PIN);
|
||||
init_motor_pwm(MOTOR_REV_PIN);
|
||||
LOG(1, "Initializing ADC...")
|
||||
init_adc();
|
||||
LOG(1, "Initializing Outputs...\n");
|
||||
init_outputs();
|
||||
LOG(1, "Initializing GPIO...\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, "core0 Initialized!\n");
|
||||
while (true); //sleep_ms(100); //TODO: any potential issues when using sleep?
|
||||
}
|
||||
@@ -0,0 +1,190 @@
|
||||
/*!
|
||||
*
|
||||
* \file core0.h
|
||||
* Used exclusively by core0
|
||||
*
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#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
|
||||
#define MESSAGE_MASK_4_BYTES 0b11111111111000000001000000001000000001000000001
|
||||
#define MESSAGE_5_BYTES 0b11111111110000000000000000000000000000000000000000000001
|
||||
#define MESSAGE_MASK_5_BYTES 0b11111111111000000001000000001000000001000000001000000001
|
||||
|
||||
#define FLASH_CMD_READ_JEDEC_ID 0x9F
|
||||
|
||||
/*!
|
||||
* \brief Bitmasks used for clearing bits corresponding to function blocks
|
||||
*
|
||||
* [0] = F0-F4
|
||||
* [1] = F5-F8
|
||||
* [2] = F9-F12
|
||||
* [3] = F13-F20
|
||||
* [4] = F21-F28
|
||||
* [5] = F29-F31
|
||||
*
|
||||
*/
|
||||
const uint32_t clr_bit_arr[6] = {
|
||||
0b11111111111111111111111111100000, // Clear Bits 0-4 (F0-F4)
|
||||
0b11111111111111111111111000011111, // Clear Bits 5-8 (F5-F8)
|
||||
0b11111111111111111110000111111111, // Clear Bits 9-12 (F9-F12)
|
||||
0b11111111111000000001111111111111, // Clear Bits 13-20 (F13-F20)
|
||||
0b11100000000111111111111111111111, // Clear Bits 21-28 (F21-F28)
|
||||
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.
|
||||
*/
|
||||
float two_std_dev(const float arr[], uint32_t length);
|
||||
|
||||
/*!
|
||||
* \brief Measures ADC offset and saves the offset value to flash memory.
|
||||
* \param n Amount of measurements to be done
|
||||
*/
|
||||
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
|
||||
*/
|
||||
void acknowledge();
|
||||
|
||||
/*!
|
||||
* \brief Function for verifying a single bit of a CV
|
||||
* \param cv_address CV Address
|
||||
* \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);
|
||||
|
||||
/*!
|
||||
* \brief Function for verifying a CV byte
|
||||
* \param cv_address CV Address
|
||||
* \param cv_data Byte to be verified
|
||||
*/
|
||||
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
|
||||
*/
|
||||
void write_cv_byte(uint16_t cv_address, uint8_t cv_data);
|
||||
|
||||
/*!
|
||||
* \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>
|
||||
*
|
||||
* Procedure:
|
||||
* 1. Checks for valid programming command
|
||||
* 2. Disables core1 timers
|
||||
* 3. Resets core1
|
||||
* 4. Saves interrupts
|
||||
* 5. Evaluates the type of programming instruction Write byte / Verify byte / Verify bit and runs corresponding function
|
||||
* 6. Restores interrupts
|
||||
* 7. Launch core1
|
||||
*
|
||||
* \param number_of_bytes Number of bytes in byte_array[]
|
||||
* \param byte_array Pointer to byte_array[]
|
||||
*/
|
||||
void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
|
||||
/*!
|
||||
* \brief Set outputs according to function mapping in CV257 to CV512
|
||||
*
|
||||
* This function maps the functions and direction to the mapped outputs configured in CV257 to CV512.
|
||||
* Also enables PWM when enabled according to CV112 to CV115.
|
||||
*
|
||||
* \param functions_to_set_bitmask Function bitmask F0 is bit0, F1 is bit1, ...
|
||||
*/
|
||||
void set_outputs(uint32_t functions_to_set_bitmask);
|
||||
|
||||
void update_active_functions(uint32_t new_function_bitmask, uint8_t clr_bit_ind, bool direction_change);
|
||||
|
||||
bool error_detection(int8_t number_of_bytes, const uint8_t *byte_array);
|
||||
|
||||
bool is_long_address(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
|
||||
bool address_evaluation(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
|
||||
void instruction_evaluation(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
|
||||
bool reset_message_check(uint8_t number_of_bytes, const uint8_t *const byte_array);
|
||||
|
||||
int8_t verify_dcc_message();
|
||||
|
||||
void bits_to_byte_array(int8_t number_of_bytes, uint8_t byte_array[]);
|
||||
|
||||
void evaluate_message();
|
||||
|
||||
/*!
|
||||
* \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);
|
||||
|
||||
/*!
|
||||
* \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.
|
||||
* 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.
|
||||
*
|
||||
* \param gpio Unused parameter
|
||||
* \param events Unused parameter
|
||||
*/
|
||||
void track_signal_fall(unsigned int gpio, long unsigned int events);
|
||||
|
||||
/*!
|
||||
* \brief Output initialization function
|
||||
*
|
||||
* Initializes all allowed GPIO outputs as outputs <br>
|
||||
* 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();
|
||||
|
||||
/*!
|
||||
* \brief Checks various CV values in order to detect factory condition or pending calibrations
|
||||
*
|
||||
* Procedure:
|
||||
* 1. Check for flash memory factory condition when found write default values specified in CV.h to flash memory
|
||||
* 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();
|
||||
|
||||
/*!
|
||||
* \brief Function initializes motor PWM for pin specified
|
||||
* \param gpio GPIO pin
|
||||
*/
|
||||
void init_motor_pwm(uint8_t gpio);
|
||||
|
||||
/*!
|
||||
* \brief Initializes ADC
|
||||
*
|
||||
* Function initializes ADC, corresponding GPIO pins and ADC FIFO
|
||||
*/
|
||||
void init_adc();
|
||||
@@ -0,0 +1,330 @@
|
||||
//////////////////////////
|
||||
// RP2040-Decoder //
|
||||
// Gabriel Koppenstein //
|
||||
// core1.c //
|
||||
//////////////////////////
|
||||
|
||||
#include "core1.h"
|
||||
|
||||
struct repeating_timer pid_control_timer, speed_helper_timer;
|
||||
|
||||
|
||||
// Get speed_step_table_index depending on speed_step
|
||||
uint8_t get_speed_step_table_index_of_speed_step(uint8_t speed_step) {
|
||||
// Clear direction information in bit7
|
||||
speed_step = speed_step & 0b01111111;
|
||||
// Check for Stop or Emergency Stop speed step
|
||||
if (speed_step == 0 || speed_step == 1) {
|
||||
return 0;
|
||||
}
|
||||
// In any other case i.e. speed steps 1 to 126:
|
||||
return speed_step-1;
|
||||
}
|
||||
|
||||
|
||||
// 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) {
|
||||
// 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;
|
||||
static uint8_t speed_table_index;
|
||||
|
||||
// Get speed_step_table_end_index corresponding to current speed_step_target
|
||||
const uint8_t speed_step_table_end_index = get_speed_step_table_index_of_speed_step(speed_step_target);
|
||||
const bool direction_changed = get_direction_of_speed_step(speed_step_target) !=
|
||||
get_direction_of_speed_step(speed_step_target_prev);
|
||||
|
||||
if (speed_step_target == 129 || speed_step_target == 1) {
|
||||
// Emergency Stop
|
||||
speed_table_index = 0;
|
||||
speed_helper_counter = 0;
|
||||
ctrl_par->setpoint = ctrl_par->speed_table[speed_table_index];
|
||||
}
|
||||
else if (speed_step_table_end_index > speed_table_index && speed_helper_counter == accel_rate) {
|
||||
// Acceleration
|
||||
if (!accel_rate) speed_table_index = speed_step_table_end_index; // directly accelerate to end target speed
|
||||
else speed_table_index++;
|
||||
ctrl_par->setpoint = ctrl_par->speed_table[speed_table_index];
|
||||
speed_helper_counter = 0;
|
||||
}
|
||||
else if (speed_step_table_end_index < speed_table_index && speed_helper_counter == decel_rate) {
|
||||
// Deceleration
|
||||
if (!decel_rate) speed_table_index = speed_step_table_end_index; // directly decelerate to end target speed
|
||||
else speed_table_index--;
|
||||
ctrl_par->setpoint = ctrl_par->speed_table[speed_table_index];
|
||||
speed_helper_counter = 0;
|
||||
}
|
||||
else if (direction_changed) {
|
||||
// Change of direction while decelerating -> jump to most recent speed_step without delay (other direction)
|
||||
speed_table_index = speed_step_table_end_index;
|
||||
ctrl_par->setpoint = ctrl_par->speed_table[speed_table_index];
|
||||
speed_helper_counter = 0;
|
||||
}
|
||||
else if (speed_step_table_end_index != speed_table_index) {
|
||||
// Acceleration/Deceleration "scheduled"
|
||||
speed_helper_counter++;
|
||||
}
|
||||
else {
|
||||
// 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.
|
||||
void adjust_pwm_level(const uint16_t level) {
|
||||
if (get_direction_of_speed_step(speed_step_target)) {
|
||||
// Forward
|
||||
pwm_set_gpio_level(MOTOR_REV_PIN, 0);
|
||||
pwm_set_gpio_level(MOTOR_FWD_PIN, level);
|
||||
}
|
||||
else {
|
||||
// Reverse
|
||||
pwm_set_gpio_level(MOTOR_FWD_PIN, 0);
|
||||
pwm_set_gpio_level(MOTOR_REV_PIN, level);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Returns proportional gain value corresponding to current setpoint
|
||||
float get_kp(const controller_parameter_t *const ctrl_par) {
|
||||
const float sp = (float) ctrl_par->setpoint;
|
||||
if (sp < ctrl_par->pid.k_p_x_1) {
|
||||
return ctrl_par->pid.k_p_m_1 * sp + ctrl_par->pid.k_p_y_0;
|
||||
}
|
||||
return ctrl_par->pid.k_p_m_2 * (sp - ctrl_par->pid.k_p_x_1) + ctrl_par->pid.k_p_y_1;
|
||||
}
|
||||
|
||||
|
||||
uint16_t get_initial_level(controller_parameter_t *const ctrl_par){
|
||||
uint32_t sum = 0;
|
||||
uint32_t i = 0;
|
||||
while (i < 16) {
|
||||
if (ctrl_par->startup.base_pwm_arr[i] > 0) {
|
||||
sum += ctrl_par->startup.base_pwm_arr[i];
|
||||
i++;
|
||||
}
|
||||
else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
return ((sum / i)*2)/3;
|
||||
}
|
||||
|
||||
// Controller - Startup mode
|
||||
void controller_startup_mode(controller_parameter_t *const ctrl_par) {
|
||||
if(ctrl_par->startup.level == 0) {
|
||||
ctrl_par->startup.level = get_initial_level(ctrl_par);
|
||||
}
|
||||
if (ctrl_par->measurement_corrected < 7.5f){
|
||||
const uint16_t max_level = _125M / (CV_ARRAY_FLASH[8] * 100 + 10000);
|
||||
adjust_pwm_level(ctrl_par->startup.level);
|
||||
ctrl_par->startup.level += max_level / 250;
|
||||
if (ctrl_par->startup.level > max_level) {
|
||||
// Try again with half value...
|
||||
ctrl_par->startup.level = get_initial_level(ctrl_par)/2;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Save level value in array
|
||||
ctrl_par->startup.base_pwm_arr[ctrl_par->startup.base_pwm_arr_i] = ctrl_par->startup.level;
|
||||
// Update index
|
||||
ctrl_par->startup.base_pwm_arr_i = (ctrl_par->startup.base_pwm_arr_i+1)%16;
|
||||
// Set ramp_up_mode flag to true
|
||||
ctrl_par->mode = PID_MODE;
|
||||
// multiply with constant value of 0.9 and set as current feed forward value for pid mode
|
||||
ctrl_par->feed_fwd = 0.9f * (float)ctrl_par->startup.level;
|
||||
}
|
||||
}
|
||||
|
||||
// Controller - PID control mode
|
||||
void controller_pid_mode(controller_parameter_t *const ctrl_par) {
|
||||
ctrl_par->pid.k_p = get_kp(ctrl_par);
|
||||
// Proportional part, integral part and derivative part (including digital low-pass-filter with time constant tau)
|
||||
float p = ctrl_par->pid.k_p * ctrl_par->pid.e;
|
||||
float i = ctrl_par->pid.ci_0 * (ctrl_par->pid.e + ctrl_par->pid.e_prev) + ctrl_par->pid.i_prev;
|
||||
float d = ctrl_par->pid.cd_0 * (ctrl_par->measurement - ctrl_par->measurement_prev) + ctrl_par->pid.cd_1 * ctrl_par->pid.d_prev;
|
||||
|
||||
// Check for limits on the integral part
|
||||
if (i > ctrl_par->pid.int_lim_max) {
|
||||
i = ctrl_par->pid.int_lim_max;
|
||||
}
|
||||
else if (i < ctrl_par->pid.int_lim_min) {
|
||||
i = ctrl_par->pid.int_lim_min;
|
||||
}
|
||||
|
||||
// Sum feed forward + all controller terms (p+i+d). Limit Output from 0% to 100% duty cycle
|
||||
float output_f = ctrl_par->feed_fwd + p + i + d;
|
||||
if (output_f < 0) {
|
||||
output_f = 0.0f;
|
||||
}
|
||||
else if (output_f > ctrl_par->pid.max_output) {
|
||||
output_f = ctrl_par->pid.max_output;
|
||||
}
|
||||
|
||||
if (LOGLEVEL >= 1) {
|
||||
static int counter = 0;
|
||||
counter++;
|
||||
if (counter > 100) {
|
||||
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));
|
||||
// Save previous error, integrator, differentiator values
|
||||
ctrl_par->pid.e_prev = ctrl_par->pid.e;
|
||||
ctrl_par->pid.i_prev = i;
|
||||
ctrl_par->pid.d_prev = d;
|
||||
}
|
||||
|
||||
// General controller function gets called every x milliseconds where x is CV_49 i.e. sampling time (pid->t)
|
||||
// TODO: 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);
|
||||
|
||||
// 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);
|
||||
// When setpoint == 0 there is nothing to control -> set output to 0, reset values and return
|
||||
if (!ctrl_par->setpoint || direction_changed) {
|
||||
ctrl_par->pid.d_prev = 0.0f;
|
||||
ctrl_par->pid.i_prev = 0.0f;
|
||||
ctrl_par->pid.e_prev = 0.0f;
|
||||
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;
|
||||
}
|
||||
|
||||
// Measure BEMF voltage and compute error
|
||||
ctrl_par->measurement = measure(ctrl_par->msr_total_iterations,
|
||||
ctrl_par->msr_delay_in_us,
|
||||
ctrl_par->l_side_arr_cutoff,
|
||||
ctrl_par->r_side_arr_cutoff,
|
||||
get_direction_of_speed_step(speed_step_target));
|
||||
ctrl_par->measurement_corrected = ctrl_par->measurement - ctrl_par->adc_offset;
|
||||
ctrl_par->pid.e = (float) ctrl_par->setpoint - ctrl_par->measurement_corrected;
|
||||
|
||||
switch (ctrl_par->mode){
|
||||
case STARTUP_MODE:
|
||||
controller_startup_mode(ctrl_par);
|
||||
break;
|
||||
case PID_MODE:
|
||||
controller_pid_mode(ctrl_par);
|
||||
break;
|
||||
}
|
||||
|
||||
adc_fifo_drain();
|
||||
|
||||
// Save measurement value
|
||||
ctrl_par->measurement_prev = ctrl_par->measurement;
|
||||
|
||||
// return true to schedule the repeating timer again
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
// PID controller and measurement parameter, and speed_table initialization
|
||||
void init_controller(controller_parameter_t *const ctrl_par) {
|
||||
// General controller variables
|
||||
ctrl_par->mode = STARTUP_MODE;
|
||||
ctrl_par->feed_fwd = 0;
|
||||
|
||||
// Startup controller variables
|
||||
ctrl_par->startup.level = 0;
|
||||
ctrl_par->startup.base_pwm_arr_i = 0;
|
||||
for (int i = 0; i < BASE_PWM_ARR_LEN; ++i) {
|
||||
ctrl_par->startup.base_pwm_arr[i] = 0;
|
||||
}
|
||||
|
||||
// Measurement variables initialization
|
||||
ctrl_par->adc_offset = (float) CV_ARRAY_FLASH[171];
|
||||
ctrl_par->msr_total_iterations = CV_ARRAY_FLASH[60];
|
||||
ctrl_par->msr_delay_in_us = CV_ARRAY_FLASH[61];
|
||||
ctrl_par->l_side_arr_cutoff = CV_ARRAY_FLASH[62];
|
||||
ctrl_par->r_side_arr_cutoff = CV_ARRAY_FLASH[63];
|
||||
|
||||
// Speed table initialization
|
||||
// Calculate speed setpoint table according to V_min, V_max and V_mid CVs
|
||||
const double v_min = (double) CV_ARRAY_FLASH[1];
|
||||
const double v_mid = (double) CV_ARRAY_FLASH[5] * 16;
|
||||
const double v_max = (double) CV_ARRAY_FLASH[4] * 16;
|
||||
|
||||
LOG(1, "speedtable min(%f) mid(%f) max(%f)\n", v_min, v_mid, v_max);
|
||||
|
||||
const double delta_x = 63;
|
||||
const double m_1 = (v_mid - v_min) / delta_x;
|
||||
const double m_2 = (v_max - v_mid) / delta_x;
|
||||
ctrl_par->speed_table[0] = 0;
|
||||
for (uint8_t i = 1; i < 64; ++i) {
|
||||
ctrl_par->speed_table[i] = (uint16_t) lround(m_1 * (i - 1) + v_min);
|
||||
}
|
||||
for (uint8_t i = 64; i < 127; ++i) {
|
||||
ctrl_par->speed_table[i] = (uint16_t) lround(m_2 * (i - 63) + v_mid);
|
||||
}
|
||||
|
||||
// PID Controller initialization
|
||||
ctrl_par->pid.k_i = (float) CV_ARRAY_FLASH[49] / 10;
|
||||
ctrl_par->pid.k_d = (float) CV_ARRAY_FLASH[50] / 10000;
|
||||
ctrl_par->pid.tau = (float) CV_ARRAY_FLASH[47] / 1000;
|
||||
ctrl_par->pid.t = (float) CV_ARRAY_FLASH[48] / 1000;
|
||||
ctrl_par->pid.ci_0 = (ctrl_par->pid.k_i * ctrl_par->pid.t) / 2;
|
||||
ctrl_par->pid.cd_0 = -(2 * ctrl_par->pid.k_d) / (2 * ctrl_par->pid.tau + ctrl_par->pid.t);
|
||||
ctrl_par->pid.cd_1 = (2 * ctrl_par->pid.tau - ctrl_par->pid.t) / (2 * ctrl_par->pid.tau + ctrl_par->pid.t);
|
||||
ctrl_par->pid.int_lim_max = 10 * (float) CV_ARRAY_FLASH[51];
|
||||
ctrl_par->pid.int_lim_min = -10 * (float) CV_ARRAY_FLASH[52];
|
||||
ctrl_par->pid.max_output = (float) (_125M / (CV_ARRAY_FLASH[8] * 100 + 10000));
|
||||
ctrl_par->pid.e_prev = 0.0f;
|
||||
ctrl_par->pid.i_prev = 0.0f;
|
||||
ctrl_par->pid.d_prev = 0.0f;
|
||||
ctrl_par->pid.k_p_x_1_shift = (float) CV_ARRAY_FLASH[59] / 255.0f;
|
||||
ctrl_par->pid.k_p_x_1 = (float) ctrl_par->speed_table[126] * ctrl_par->pid.k_p_x_1_shift;
|
||||
ctrl_par->pid.k_p_x_2 = (float) ctrl_par->speed_table[126] * (1.0f - ctrl_par->pid.k_p_x_1_shift);
|
||||
ctrl_par->pid.k_p_y_0 = (float) get_16bit_CV(53) / 100.0f;
|
||||
ctrl_par->pid.k_p_y_1 = (float) get_16bit_CV(55) / 100.0f;
|
||||
ctrl_par->pid.k_p_y_2 = (float) get_16bit_CV(57) / 100.0f;
|
||||
ctrl_par->pid.k_p_m_1 = (ctrl_par->pid.k_p_y_1 - ctrl_par->pid.k_p_y_0) / ctrl_par->pid.k_p_x_1;
|
||||
ctrl_par->pid.k_p_m_2 = (ctrl_par->pid.k_p_y_2 - ctrl_par->pid.k_p_y_1) / ctrl_par->pid.k_p_x_2;
|
||||
|
||||
// General Controller initialization
|
||||
ctrl_par->measurement = 0.0f;
|
||||
ctrl_par->measurement_prev = 0.0f;
|
||||
ctrl_par->measurement_corrected = 0.0f;
|
||||
ctrl_par->setpoint = 0;
|
||||
|
||||
}
|
||||
|
||||
void core1_entry() {
|
||||
LOG(1, "core1 Initialization...\n");
|
||||
if (flash_safe_execute_core_init() != true){
|
||||
set_error(FLASH_SAFE_EXECUTE_CORE_INIT_FAILURE);
|
||||
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,
|
||||
ctrl_par,
|
||||
&pid_control_timer);
|
||||
// Set TIMER_IRQ_0 to highest priority of 0x00
|
||||
irq_set_priority(TIMER_IRQ_0, 0x00);
|
||||
alarm_pool_add_repeating_timer_ms(alarm_pool_create(1, 1),
|
||||
CV_ARRAY_FLASH[174],
|
||||
speed_helper,
|
||||
ctrl_par,
|
||||
&speed_helper_timer);
|
||||
LOG(1, "core1 Initialized!\n");
|
||||
while (true); //sleep_ms(100); //TODO: any potential issues when using sleep?
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
/*!
|
||||
*
|
||||
* \file core1.h
|
||||
* Used exclusively by core1
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "shared.h"
|
||||
|
||||
#define BASE_PWM_ARR_LEN 16
|
||||
|
||||
typedef enum {
|
||||
STARTUP_MODE,
|
||||
PID_MODE,
|
||||
} controller_mode_t;
|
||||
|
||||
typedef struct startup_parameters_t {
|
||||
uint16_t level;
|
||||
uint16_t base_pwm_arr[BASE_PWM_ARR_LEN];
|
||||
uint16_t base_pwm_arr_i;
|
||||
}startup_parameters_t;
|
||||
|
||||
typedef struct pid_parameters_t{
|
||||
// PID Controller variables
|
||||
float k_i; // Integral gain
|
||||
float k_d; // Derivative gain
|
||||
float t; // Sampling time
|
||||
float tau; // Low-pass-filter time constant
|
||||
float ci_0; // (k_i*t)/2
|
||||
float cd_0; // (k_d*2)/(2*tau+t)
|
||||
float cd_1; // (2*tau-t)/(2*tau+t)
|
||||
float int_lim_max; // max limit for integrator
|
||||
float int_lim_min; // min limit for integrator
|
||||
float max_output; // max possible pwm output value
|
||||
float k_p; // Proportional gain
|
||||
float e; // Error
|
||||
float e_prev; // Previous Error
|
||||
float i_prev; // Previous Integral Value
|
||||
float d_prev; // Previous Derivative Value
|
||||
// Variable gain variables
|
||||
float k_p_x_1_shift;
|
||||
float k_p_x_1;
|
||||
float k_p_x_2;
|
||||
float k_p_y_0;
|
||||
float k_p_y_1;
|
||||
float k_p_y_2;
|
||||
float k_p_m_1;
|
||||
float k_p_m_2;
|
||||
} pid_parameter_t;
|
||||
|
||||
typedef struct controller_parameter_t{
|
||||
/*!
|
||||
* General Variables
|
||||
*/
|
||||
controller_mode_t mode; // Controller mode
|
||||
float feed_fwd; // Feed forward value
|
||||
uint32_t setpoint; // Current setpoint
|
||||
uint16_t speed_table[127]; // Array with setpoint values corresponding to every speed step
|
||||
|
||||
/*!
|
||||
* Startup controller mode specific variables
|
||||
*/
|
||||
startup_parameters_t startup;
|
||||
|
||||
/*!
|
||||
* PID Controller variables
|
||||
*/
|
||||
pid_parameter_t pid;
|
||||
|
||||
/*!
|
||||
* Measurement Variables
|
||||
*/
|
||||
float measurement; // Newest Measurement value
|
||||
float measurement_prev; // Previous Measurement
|
||||
float measurement_corrected; // measurement - adc_offset = measurement_corrected
|
||||
float adc_offset; // ADC offset value
|
||||
uint8_t msr_delay_in_us; // Delay before VEMF Measurement
|
||||
uint8_t msr_total_iterations; // Amount of samples
|
||||
uint8_t l_side_arr_cutoff; // Discarded samples (left side)
|
||||
uint8_t r_side_arr_cutoff; // Discarded samples (right side)
|
||||
|
||||
} controller_parameter_t;
|
||||
|
||||
|
||||
|
||||
uint8_t get_speed_step_table_index_of_speed_step(uint8_t speed_step);
|
||||
bool speed_helper(struct repeating_timer *t);
|
||||
void adjust_pwm_level(uint16_t level);
|
||||
float get_kp(const controller_parameter_t *ctrl_par);
|
||||
uint16_t get_initial_level(controller_parameter_t * ctrl_par);
|
||||
void controller_startup_mode(controller_parameter_t * ctrl_par);
|
||||
bool controller_general(struct repeating_timer * t);
|
||||
void init_controller(controller_parameter_t * ctrl_par);
|
||||
@@ -0,0 +1,84 @@
|
||||
# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
|
||||
|
||||
# This can be dropped into an external project to help locate this SDK
|
||||
# It should be include()ed prior to project()
|
||||
|
||||
if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
|
||||
set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
|
||||
message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
|
||||
endif ()
|
||||
|
||||
if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
|
||||
set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
|
||||
message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
|
||||
endif ()
|
||||
|
||||
if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
|
||||
set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
|
||||
message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
|
||||
endif ()
|
||||
|
||||
if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_TAG} AND (NOT PICO_SDK_FETCH_FROM_GIT_TAG))
|
||||
set(PICO_SDK_FETCH_FROM_GIT_TAG $ENV{PICO_SDK_FETCH_FROM_GIT_TAG})
|
||||
message("Using PICO_SDK_FETCH_FROM_GIT_TAG from environment ('${PICO_SDK_FETCH_FROM_GIT_TAG}')")
|
||||
endif ()
|
||||
|
||||
if (PICO_SDK_FETCH_FROM_GIT AND NOT PICO_SDK_FETCH_FROM_GIT_TAG)
|
||||
set(PICO_SDK_FETCH_FROM_GIT_TAG "master")
|
||||
message("Using master as default value for PICO_SDK_FETCH_FROM_GIT_TAG")
|
||||
endif()
|
||||
|
||||
set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
|
||||
set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
|
||||
set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
|
||||
set(PICO_SDK_FETCH_FROM_GIT_TAG "${PICO_SDK_FETCH_FROM_GIT_TAG}" CACHE FILEPATH "release tag for SDK")
|
||||
|
||||
if (NOT PICO_SDK_PATH)
|
||||
if (PICO_SDK_FETCH_FROM_GIT)
|
||||
include(FetchContent)
|
||||
set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
|
||||
if (PICO_SDK_FETCH_FROM_GIT_PATH)
|
||||
get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
|
||||
endif ()
|
||||
# GIT_SUBMODULES_RECURSE was added in 3.17
|
||||
if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
|
||||
FetchContent_Declare(
|
||||
pico_sdk
|
||||
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
|
||||
GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
|
||||
GIT_SUBMODULES_RECURSE FALSE
|
||||
)
|
||||
else ()
|
||||
FetchContent_Declare(
|
||||
pico_sdk
|
||||
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
|
||||
GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
|
||||
)
|
||||
endif ()
|
||||
|
||||
if (NOT pico_sdk)
|
||||
message("Downloading Raspberry Pi Pico SDK")
|
||||
FetchContent_Populate(pico_sdk)
|
||||
set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
|
||||
endif ()
|
||||
set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
|
||||
else ()
|
||||
message(FATAL_ERROR
|
||||
"SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
|
||||
)
|
||||
endif ()
|
||||
endif ()
|
||||
|
||||
get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
|
||||
if (NOT EXISTS ${PICO_SDK_PATH})
|
||||
message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
|
||||
endif ()
|
||||
|
||||
set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
|
||||
if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
|
||||
message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
|
||||
endif ()
|
||||
|
||||
set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
|
||||
|
||||
include(${PICO_SDK_INIT_CMAKE_FILE})
|
||||
@@ -0,0 +1,85 @@
|
||||
//////////////////////////
|
||||
// RP2040-Decoder //
|
||||
// Gabriel Koppenstein //
|
||||
// shared.c //
|
||||
//////////////////////////
|
||||
|
||||
#include "shared.h"
|
||||
|
||||
uint8_t speed_step_target = 0;
|
||||
uint8_t speed_step_target_prev = 0;
|
||||
|
||||
uint32_t error_state = 0;
|
||||
|
||||
|
||||
// Functions in shared.c can be accessed by both cores
|
||||
// Measures Back-EMF voltage (proportional to the rotational speed of the motor) on GPIO 28 and GPIO 29 respectively (depending on direction)
|
||||
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){
|
||||
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);
|
||||
busy_wait_us(measurement_delay_us);
|
||||
adc_run(true);
|
||||
for (int32_t i = 0; i < total_iterations; ++i) {
|
||||
//Take adc value out of fifo when available, then do one step of insertion sort.
|
||||
adc_val[i] = adc_fifo_get_blocking();
|
||||
key = adc_val[i];
|
||||
j = i - 1;
|
||||
while (j >= 0 && adc_val[j] > key) {
|
||||
adc_val[j + 1] = adc_val[j];
|
||||
j--;
|
||||
}
|
||||
adc_val[j + 1] = key;
|
||||
}
|
||||
adc_run(false);
|
||||
//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) {
|
||||
sum += adc_val[i];
|
||||
}
|
||||
float res = (float)sum / (float)( total_iterations - (l_side_arr_cutoff + r_side_arr_cutoff) );
|
||||
// return true to keep the repeating timer active
|
||||
return res;
|
||||
}
|
||||
|
||||
// retrieve 4Bytes from CV_ARRAY_FLASH at once and return 32-Bit value
|
||||
uint32_t get_32bit_CV (uint16_t CV_start_index){
|
||||
uint8_t byte_3 = CV_ARRAY_FLASH[CV_start_index];
|
||||
uint8_t byte_2 = CV_ARRAY_FLASH[CV_start_index+1];
|
||||
uint8_t byte_1 = CV_ARRAY_FLASH[CV_start_index+2];
|
||||
uint8_t byte_0 = CV_ARRAY_FLASH[CV_start_index+3];
|
||||
return (byte_0) + (byte_1<<8) + (byte_2<<16) + (byte_3<<24);
|
||||
}
|
||||
|
||||
|
||||
// retrieve 2Bytes from CV_ARRAY_FLASH at once and return 16-Bit value
|
||||
uint16_t get_16bit_CV (uint16_t CV_start_index){
|
||||
uint8_t byte_1 = CV_ARRAY_FLASH[CV_start_index];
|
||||
uint8_t byte_0 = CV_ARRAY_FLASH[CV_start_index+1];
|
||||
return (byte_0) + (byte_1<<8);
|
||||
}
|
||||
|
||||
bool get_direction_of_speed_step(uint8_t speed_step){
|
||||
// Shift by 7 Bytes to move bit7 into bit0 position and return
|
||||
return speed_step >> 7;
|
||||
}
|
||||
|
||||
void set_error(error_t err){
|
||||
error_state |= err;
|
||||
}
|
||||
|
||||
void clear_error(error_t err){
|
||||
error_state & (~err);
|
||||
}
|
||||
|
||||
uint32_t get_error_state(){
|
||||
return error_state;
|
||||
}
|
||||
@@ -0,0 +1,119 @@
|
||||
/*!
|
||||
*
|
||||
* \file shared.h
|
||||
* Used by both cores
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "string.h"
|
||||
#include "pico/float.h"
|
||||
#include "pico/stdlib.h"
|
||||
#include "pico/multicore.h"
|
||||
#include "pico/printf.h"
|
||||
#include "pico/flash.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include "hardware/adc.h"
|
||||
#include "hardware/flash.h"
|
||||
#include "hardware/gpio.h"
|
||||
|
||||
typedef enum {
|
||||
FLASH_SAFE_EXECUTE_CORE_INIT_FAILURE = (1<<0),
|
||||
FLASH_SAFE_EXECUTE_ERASE_FAILURE = (1<<1),
|
||||
FLASH_SAFE_EXECUTE_PROGRAM_FAILURE = (1<<2),
|
||||
FLASH_SIZE_READBACK_FAILURE = (1<<3),
|
||||
STDIO_INIT_FAILURE = (1<<4),
|
||||
} error_t;
|
||||
|
||||
|
||||
// here you can enable printf debugging (needs extra soldering and you have to enable uart in the cmake configuration)
|
||||
// please note
|
||||
// - if it's disabled it will be compiled completely out
|
||||
// - be careful that printf debugging can change your timing behavior, for example in the pid controller limit the outputs
|
||||
// like already done (only print every 100th iteration for example!)
|
||||
// - also note that with logging enabled GPIO0/1 are DISABLED from decoder functions because they are used for uart to the
|
||||
// outside world (this is configure automatically for you below!)
|
||||
// - IMPORTANT: you have to enable uart in the cmakelists.txt to enable logging!
|
||||
#define LOGLEVEL 1
|
||||
#define LOG(level, ...) { if(level <= LOGLEVEL) {printf(__VA_ARGS__);}}
|
||||
|
||||
// Constant Value of 125 x 10^6
|
||||
#define _125M 125000000
|
||||
|
||||
// GPIO used for DCC-Signal, Motor PWM or ADC
|
||||
#define DCC_INPUT_PIN 21u
|
||||
#define MOTOR_FWD_PIN 23u
|
||||
#define MOTOR_REV_PIN 22u
|
||||
#define FWD_V_EMF_ADC_PIN 28u
|
||||
#define REV_V_EMF_ADC_PIN 29u
|
||||
|
||||
// if logging is enabled do NOT use gpio0/1 for functions in the decoder
|
||||
#if LOGLEVEL != 0
|
||||
// GPIO used directly (GPIO 2-5 incl.) as outputs or to switch auxiliary output transistors (GPIO 24-27 incl.)
|
||||
#define GPIO_OUTPUT_PIN_MASK (1u<<24) | (1u<<25) | (1u<<26) | (1u<<27) | (1u<<5) | (1u<<4) | (1u<<3) | (1u<<2)
|
||||
#else
|
||||
// GPIO used directly (GPIO 0-5 incl.) as outputs or to switch auxiliary output transistors (GPIO 24-27 incl.)
|
||||
#define GPIO_OUTPUT_PIN_MASK (1u<<24) | (1u<<25) | (1u<<26) | (1u<<27) | (1u<<5) | (1u<<4) | (1u<<3) | (1u<<2) | (1u<<1) | (1u<<0)
|
||||
#endif
|
||||
|
||||
// GPIO pin mask to prevent setting illegal GPIOs (ADC, Motor, DCC Input Pin)
|
||||
#define GPIO_ILLEGAL_MASK (1u<<DCC_INPUT_PIN) | (1u<<MOTOR_FWD_PIN) | (1u<<MOTOR_REV_PIN) | (1u<<FWD_V_EMF_ADC_PIN) | (1u<<REV_V_EMF_ADC_PIN)
|
||||
|
||||
// GPIO pin mask with allowed outputs (AUX & GPIO (configured as outputs))
|
||||
#define GPIO_ALLOWED_OUTPUTS (GPIO_OUTPUT_PIN_MASK) & ~(GPIO_ILLEGAL_MASK)
|
||||
|
||||
|
||||
extern repeating_timer_t pid_control_timer;
|
||||
extern repeating_timer_t speed_helper_timer;
|
||||
extern const uint8_t *CV_ARRAY_FLASH;
|
||||
extern uint8_t speed_step_target;
|
||||
extern uint8_t speed_step_target_prev;
|
||||
|
||||
|
||||
void core1_entry();
|
||||
void write_cv_handler(uint16_t cv_index, uint8_t cv_data);
|
||||
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);
|
||||
uint32_t get_32bit_CV (uint16_t CV_start_index);
|
||||
uint16_t get_16bit_CV (uint16_t CV_start_index);
|
||||
/*!
|
||||
* @brief Calculates the direction based on a speed step byte.
|
||||
*
|
||||
* This function calculates the direction of movement based on the value of
|
||||
* the `speed_step` byte. It refers to the "128 Speed Control" standard as outlined
|
||||
* 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 |
|
||||
*
|
||||
* @param speed_step The speed step byte.
|
||||
* @return `true` for forward direction, `false` for reverse direction.
|
||||
*/
|
||||
bool get_direction_of_speed_step(uint8_t speed_step);
|
||||
|
||||
void set_error(error_t err);
|
||||
|
||||
void clear_error(error_t err);
|
||||
|
||||
uint32_t get_error_state();
|
||||
Reference in new issue
Block a user