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:
gab-k committed 2024-12-22 11:39:03 +01:00
1 parent e8a8ac6d98
commit 7b1d5ed152
12 files changed
+1452 -1271

No files matched your search

+1
View File
@@ -0,0 +1 @@
build
+75
View File
@@ -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
View File
File diff suppressed because it is too large. Load diff
+63
View File
@@ -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
+675
View File
@@ -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?
}
+190
View File
@@ -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();
+330
View File
@@ -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?
}
+94
View File
@@ -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);
+84
View File
@@ -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})
+85
View 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;
}
+119
View File
@@ -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();